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ABSTRACT 


A  sliding  mode  compensator  for  depth  control  of  an  autonomous  underwater 
vehicle  (AUV)  using  depth  feedback  only  is  designed.  The  controller  is  evaluated 
for  a  nominal  linear  model  and  optimized  by  a  series  of  numerical  experiments 
for  a  number  of  depth  changing  maneuvers.  A  state  observer  is  used  in  order  to 
estimate  the  unmeasurable  states  together  with  the  sliding  mode  controller.  The 
effects  of  varying  control  parameters  are  discussed.  Compensator  performance 
is  assessed  by  numerical  simulation  of  AUV  dynamic  response  based  on  the  full 
six  degrees  of  freedom  nonlinear  equations  of  motion.  The  expected  robustness 
of  the  design  is  demonstrated  by  comparison  between  linear  and  nonlinear  vehicle 
response  characteristics,  and  by  a  wide  variation  in  vehicle  parameters  and 
hydrodynamic  coefficients.  Finally,  suggestions  for  design  improvement  and  di¬ 
rections  for  future  research  are  indicated. 
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I.  INTRODUCTION 


A.  GENERAL 

There  has  been  an  increased  interest  recently  in  the  need  for  autonomous 
underwater  vehicles  (AUV)  in  both  Navy  and  private  industry.  A  variety  of  un¬ 
classified  missions  .ncludes  ASW,  decoy,  survey,  reconnaissance,  and  ocean  engi¬ 
neering  work  service.  As  the  cost  of  manned  submarine  vehicles  increases,  there 
are  significant  advantages  to  the  use  of  cheaper  unmanned  vehicles.  The  AUV 
should  be  able  to  maneuver  freely  in  the  ocean  enviroment  with  respect  to  depth, 
heading,  and  speed  in  order  to  carry  out  its  missions.  Such  maneuvering  require¬ 
ments  have  to  be  easily  accomplished  by  a  low  level  active  control  system,  and  in 
the  presence  of  environmental  and  physical  uncertainty. 

All  information  concerning  the  environment  of  a  vehicle  is  detected  by  the  sens¬ 
ing  level  of  control  on-board  the  vehicle  and  directed  to  the  high  level  intelligent 
system  in  order  to  carry  out  an  unmanned  mission.  The  dynamics  of  underwater 
vehicles  are  described  by  highly  nonlinear  systems  with  uncertain  coefficients  and 
disturbances  that  are  difficult  to  measure.  Robust  control  using  variable  structure 
systems  are  reputed  to  provide  accurate  control  of  nonlinear  systems  despite  un¬ 
modeled  system  dynamics  and  disturbances,  leading  to  the  motion  that  sliding 
mode  compensators  should  be  employed  in  situations  where  accurate  tracking  is 
desired  and  where  maneuvering  parameters  of  AUV  change  with  operating  con¬ 
ditions. 

B.  AIM  OF  THIS  STUDY 

This  thesis  aims  at  investigation  of  the  use  of  sliding  mode  compensator  for 
AUV  depth  keeping  and  changing.  The  control  concept  developed  here  is  that 
of  a  variable  structure  system  consisting  of  continuous  subsystems  together  with 
suitable  switching  logic.  The  sliding  mode  control  concept  was  suggested  by  V. 
Utkin  [Ref.  1]  and  recently  developed  by  J.J.E.  Slotine  [Ref.  2\.  Because  sliding 
mode  control  requires  full  state  feedback,  this  work  has  incorporated  a  state  ob¬ 
served  based  on  output  measurement  resulting  in  a  sliding  mode  compensator. 


The  main  goal  of  this  thesis  is  to  present  a  design  procedure  and  estimate 
robustness  of  the  variable  structure  compensator  in  the  presence  of  vehicle  non- 
linearities,  modeling  errois,  uncertainties,  and  variation  of  parameters. 

C.  THESIS  OUTLINE 

Chapter  2  introduces  the  basic  concept  of  Liapunov  stability  and  an 
asymptotically  stable  condition  which  is  related  to  energy  degeneration  with  in¬ 
creasing  time  for  a  dynamics  system.  The  other  sections  will  discuss  how  to  de¬ 
sign  a  sliding  surface  and  a  control  law  based  on  a  linear  model.  The  last  section 
of  the  chapter  presents  a  technique  to  eliminate  chattering  in  order  to  provide 
smooth  control  inputs. 

In  Chapter  3,  vehicle  dynamics  and  a  process  used  to  produce  a  linear  state 
space  representation  are  described.  Sliding  control  law  for  a  linear  model  is  de¬ 
signed  using  results  of  the  previous  chapter.  The  design  is  evaluated  through 
computer  simulation. 

Chapter  4  presents  the  sliding  mode  compensator  using  depth  measurements 
only.  A  model  with  perturbed  hydrodynamic  and  geometric  parameters  is  used 
to  estimate  the  performance  of  the  sliding  mode  compensator.  The  chapter  ends 
with  a  discussion  of  the  robustness  of  the  sliding  mode  compensator  and  advan¬ 
tage  of  the  variable  structure  control  system 

Finally,  Chapter  5  contains  a  summary,  conclusions,  and  some  directions  for 
further  research. 
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II.  THEORETICAL  BACKGROUND  OF  SLIDING  MODE  CONTROL 


A.  GENERAL 

The  dynamics  of  underwater  vehicles  are  described  by  highly  nonlinear,  high 
order  systems  with  uncertain  models  and  disturbances  that  are  difficult  to  model. 
A  new  form  of  sliding  mode  (Variable  Structure  System)  control  has  been  devel* 
oped  recently,  and  shown  to  apply  to  a  large  class  of  nonlinear  systems  [Ref.  3]. 
Sliding  mode  control  offers  the  control  designer  new  possibilities  for  improving 
the  quality  of  the  control  in  comparison  with  a  fixed  structure  system.  The  basic 
idea  is  to  design  a  controller  structure  which  consists  of  a  set  of  continuous  sub¬ 
systems  together  with  suitable  switching  logic  according  to  [Ref.  1].  The  basic 
sliding  mode  control  for  a  S1SO  system 


x  =  Ax+Bu 


w  =  [±4;x]  (2.1) 

where,  x  is  the  state  variable 

u  is  the  sliding  mode  control  law 
¥  is  a  switched  feedback  gain 
A,B  are  system  matrix. 

This  chapter  is  devoted  to  the  study  of  the  basic  background  of  the  sliding 
mode  theory  for  the  design  of  a  linear  controller  for  the  AUV. 

B.  LIAPUNOV  STABILITY 

For  a  given  control  system,  stability  is  usually  the  most  important  thing  to 
be  determined.  If  the  system  is  linear  and  time  invariant,  then,  many  stability 
criteria  are  available,  such  as  the  Nyquist  stability  criterion,  the  Routh's  stability 
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criterion  etc.  The  second  method  of  Liapunov  is  the  most  general  method  for  the 
determination  of  the  stability  of  nonlinear  and  time  varying  systems.  Before  dis¬ 
cussing  the  sliding  mode  control,  the  second  method  of  Liapunov  will  be  dis¬ 
cussed  in  order  to  understand  the  sliding  condition,  which  will  be  discussed  in  the 
next  section.  The  basic  concept  of  the  second  method  of  Liapunov  is  that  if  the 
system  has  an  asymptotically  stable  equilibrium  state,  the  stored  energy  of  system 
decays  with  increasing  time  until  it  finally  assumes  it's  minimum  value.  In  order 
to  explain  this,  Liapunov  introduced  the  so  called  Liapunov  function,  an  imagi¬ 
nary  energy  function  which  depends  on  the  state  variable  (x\,x2...x„)  and  time  (t). 
If  the  Liapunov  function  is  denoted  by  V(x,t)  and  it's  time  derivatives  denoted 
by 


V(x,t)  = 


dv(x.t ) 
dt 


(2.2) 


then  the  Liapunov  function  has  information  as  to  stability,  asymptotic  stability 
or  instability  of  an  equilibrium  state  of  the  system  without  solving  the  state 
equation.  The  theorem  of  the  Liapunov  function  is  described  in  modern  control 
engineering  [Ref.  4].  If  a  system  is  described  by 


x=Ax,t)  (2.3) 

where  x  is  the  state  variable  and  if 
f(0,t)  =  0,  for  t0  <  t. 

and  there  exists  a  Liapunov  function  V(x,t)  having  continuous  F(-V)  and  satis¬ 
fying  the  following  conditions: 


1.  V(.x,t)  is  positive  definite,  and 

2.  V(x.t)  is  negative  definite, 
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then  the  equilibrium  state  at  the  origin  is  asymptotically  stable.  F(x,r)  is 
negative  definite  which  shows  that  V(x,t)  is  continually  decreasing.  So  for  any 
system 


x=J[x,u{t)) 


(2.4) 


Q 


where,  x  is  the  state  variable 
u(t)  is  the  control  law 
x(0,t)  =*0 


and  the  time  derivative  of  the  Liapunov  function  F(x,u(r))  is  negative  definite, 
then  control  law  u(t)  is  guaranteed  stable. 

C.  DYNAMICS  OF  SYSTEM  WITH  SWITCHING 

Now  consider  the  case  that  an  asymptotically  stable  system  lr.ay  consist  of 
two  structures  neither  of  which  is  asymptotically  stable.  If  the  differential 
equations  of  the  second  order  system  have  the  following  format: 


dx 2 

~di 


■  =  Ux\ 


(2.5) 


where  u  is  a  constant  <  |  H/2 1 

then  the  structure  of  the  system  is  elliptic  as  described  in  Figure  1  on  page  6. 
Suppose  the  system  with  a  positive  feedback  gain,  then  structure  is  aperiodically 
unstable  as  shown  in  Figure  1.  The  block  diagram  of  the  closed  loop  system with 
switching  gain  is  illustrated  in  Figure  2  on  page  7.  Let  us  try  to  combine  the 
advantage  of  both  systems  by  suitable  choice  of  their  structures  in  the  appropri- 
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Figure  1.  Asymptotically  unstable  structure  |Ref.5] 

ate  parts  of  the  phase  plane.  In  order  to  get  the  asymptotically  stable  structures 
in  Figure  3  (b),  the  phase  plane  was  divided  into  four  pairwise  subsections  as 
shown  in  Figure  3  (a)  by  the  following  conditions: 

1.  Subsection  I  :  a-,  ^  0,  a^+M'a*,  >  0 

2.  Subsection  II  :  X|<0,  at2+vI/ac,  >  0 

3.  Subsection  III  :  a-,  <  0,  x2+vI'x,  <  0 

4.  Subsection  IV  :  x,  >  0,  Aj+M'x,  ^  0 

The  good  phase  trajectory  for  each  phase  portrait  has  been  chosen  to  make 
asymptotically  stable  structures  as  in  Figure  3  (b). 

This  phase  plane  is  separated  from  one  another  by  the  straight  line  x,  ==0  and 
which  we  call  the  switch  line  or  sliding  surface.  The  asymptote 


Figure  2.  The  block  diagram  with  switching  gain 


•x-j+M'-x,  =  0  acts  as  a  switching  line  for  the  structure  when  the  trajectory  of  the 
subsection  I  is  reached.  The  structure  of  the  system  can  be  switched  by  using  this 
line  instantaneously  from  elliptic  to  hyperbolic  in  this  case.  This  switching  line  is 
very  important  in  sliding  mode  control.  Once  the  system  state  trajectory  ap¬ 
proaches  the  switching  line,  and  in  order  to  keep  the  trajectory  on  the  sliding 
surface  for  t  >  r0,  then  this  system  will  become  asymptotically  stable  as  long  as  it 
satisfies  the  Liapunov  condition,  in  the  general  case,  a  switching  line  might  be  a 
straight  line,  c  -  a-2+P.Xj  (0  <  X  <  oo),  but  must  pass  through  the  origin  (i  e 
a(0)  =  0). 

D.  SLIDING  CONDITION 

Some  of  the  possible  advantages  offered  by  the  idea  of  switching  the  structure 
of  a  control  system  were  described  in  the  last  subsection.  But  we  remarked  that 
if  the  structure  does  not  change  at  the  precise  instance  when  the  trajectory  crosses 
the  switching  line,  due  to  the  effect,  noise,  then  additional  control  action  will  be 
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required  to  enforce  the  sliding  condition.  The  motion  of  the  system  now  depends 
on  sliding  surface  parameters  which  are  insensitive  to  the  external  disturbances 
and  variations  of  the  plant  parameters  .vithin  a  wide  range  of  the  switching  line. 
If  the  system  trajectory  of  the  subsection  I  crosses  into  the  switching  line  and  af¬ 
ter  passing  the  system  trajectory  of  the  subsection  II  crosses  over  the  switching 
line  repeatedly  again,  the  system  trajectory  will  be  kept  within  some  range  of  the 
switching  line.  Suppose  such  change  occurs  at  infinitely  high  frequency,  then  the 
state  of  the  system  trajectory  is  maintained  with  an  infinitesimal  amplitude  oscil¬ 
lation.  It  is  an  asymptotically  stable  system  on  this  line  as  shown  in  Figure  4  on 
page  9. 

The  motion  of  the  system  on  the  switching  line  is  described  by  the  solution 
of  the  general  differential  equation  (2.5)  [Ref.  2]  together  with  the  equation  of  the 
general  switching  line  given  by 
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cr(.v)  =  (^- =  0 


(2.6) 


where  x  =  x~xd>  a  single  element  of  the  state  vector 
X  >  0  (  arbitrary  constant) 
n  =  order  of  system 

The  motion  defined  by  the  equation  (2.6)  describes  the  system  dynamics  in  the 
sliding  mode.  A  sliding  mode  has  an  important  property  that  the  corresponding 
motion  of  the  system  depends  on  the  siiding  surface  (Switching  Line)  which  is 
chosen  by  the  designer  only.  In  order  to  know  the  mathematical  existence  condi- 
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tion  for  the  Sliding  mode,  the  theory  of  sliding  mode  has  given  a  considerable  at¬ 
tention  to  the  methods  guaranteeing  the  existence  of  the  sliding  mode. 

1.  Condition  for  Existence  of  a  Sliding  Mode 
Considering  the  general  dynamic  system 


-f  - -A**)  (2-7) 

Let  us  assume  that  the  right  hand  members  of  this  equation  are  discontinuous 
on  a  certain  sliding  surface  ct(a)  =  0  in  the  phase  space,  where,  as  phase  trajectory 
of  the  system  approaches  c7(.y)  =  0  from  either  side,  the  following  limits  are  de¬ 
fined  [Ref.  5]:, 


lim  =/"(*,  0 

<T  — *  0 


lim  J[x,t)  =J+{x,t)  (2.8) 

c->  0 

where  f~{x,t)  #/-(*,/) 


then  the  derivative  of  the  sliding  surface  {a)  along  the  trajectories  of  the  system 
is 


da 

dt 


da  d. x 
ex  dt 


where  f  =  phase  velocity  vector,  and 


(2.9) 


A-ir=(r?ra*) 
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lim  ■—  =  {/+ grade)  (2.10) 

ff-0x  at 

where  cj  is  a  smooth  function. 

At  each  point  of  a  -  0,  the  sign  of  the  limit  equation  (2.8)  has  seven  cases.  There 
is  a  case  which  is  most  interesting  as  it  corresponds  to  the  existence  of  an  ideal 
sliding  mode  on  the  sliding  surface  a  =  0,  if 

lim  4-<0 

cr-.  o"  dt 

lim  ~>0  (2.11) 

<t-o_  dt  v  ' 

The  equivalent  inequality  being  the  condition  for  existence  of  a  sliding  mode  is 

lim  a-~p-  <  0 

<7-o  dt 

or 

lim  <*  0  (2.12) 

cr-»0  dt  V  ’ 

2.  Proof  of  Stability 

This  inequality  is  also  suggested  by  [Ref.  5]  as  a  necessary  condition  for 
the  system  in  equation  (2.5)  to  have  a  degenerating  Liapunov  function  in  the 
following  form: 

PM = -flX*)]2  P-O) 
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The  asymptotic  stability  of  the  system  (2.5)  is  guaranteed  provided  that  V(x)  is 
negative  definite  as  discussed  in  the  previous  section.  If  <r(.x,/)  is  a  sliding  surface 
by  the  definition  above,  it  follows  that 

(2.14) 

where  r\  is  the  sliding  control  gain  and 
a{x,t)  is  the  sliding  surface. 

It  will  guarantee  stability  of  the  sliding  mode  motion.  The  control  law  driving 
motion  in  the  sliding  mode  can  be  obtained  by  using  this  condition  [Ref.  2].  The 
next  section  will  be  devoted  to  a  discussion  for  the  development  of  the  sliding 
mode  control  law. 

E.  SLIDING  SURFACE  DESIGN 

The  sliding  surface  has  a  very  important  property  that  is  shown  in  the  previ¬ 
ous  section.  The  sliding  surface  should  be  designed  so  that  system  response  re¬ 
stricted  to  ct(jc)  has  a  desired  behavior,  such  as  asymptotically  stable  state  or 
tracking  error. 

Let  us  consider  a  linear  time  invariant  system  to  design  the  sliding  surface 


x  =  Ax+Bu 


(2.15) 


where  x  e  Rn,  ue  Rm 

Consider  that  the  sliding  surface  of  the  equation  (2.15)  has  the  following  form: 

<r(.x)  —  STx  =  0  (2. 1 6) 


where  S  is  the  sliding  surface  coefficient  ( m  x  n) 


The  existence  of  the  sliding  mode  implies  that  c7(x)£r(x)  ^  0  and  <r(x)  =  0  for  all 
t  ^  t0.  Using  the  method  of  equivalent  control  [Ref.  6] 

i[x)=0~I?tik=sTi=a  (2-17) 

Substituting  equation  (2.15)  for  x  of  the  above  equation 

Sr{Ax+Bueq )  =  0 

or 

ueq  =  -IStBT1StAx  (2.18) 

Substituting  equation  (2.18)  to  Eq  (2.15)  and  rearranging 

or 

x  =  (2.19) 

Equation  (2.19)  gives  the  dynamics  of  the  system  on  the  sliding  surface  for  t  >:  t0 
given  a(x)  =  0,  but  the  S  matrix  is  unknown.  In  order  to  determine  S  matrix,  the 
equation  (2.19)  can  be  rearranged  in  the  following  form: 

x  =  \_A-BK^\x 

x  =  Af*  (2.20) 

where  Ke  =  ( STB)~lSTA 
Ac  =  A—BKC 

The  Kc  matrix  can  be  obtained  from  the  pole  placement  for  which  we  can  select 
specifically  desired  closed-loop  poles  of  the  system  equation  (2.20)  on  the  sliding 


surface.  If  we  get  the  Kc  matrix  by  using  the  standard  pole  placement  method,  the 
sliding  surface  matrix  {m  x  n)  can  be  determined  in  the  following  procedure: 

Kc  =  {STB)~lSTA 
StA-StBKc  =  0 

St(A-BKc)  =  StAc  =  0  (2.21) 

It  should  be  noted  that  Ac  must  be  rank  deficient  by  one  and  that  the  procedure 
must  therefore  place  one  pole  of  Ac  at  the  origin.  The  left  eigenvector  of  the  Ae 
matrix  of  equation  (2.21)  corresponding  to  a  pole  placed  at  the  origin  are  the 
sliding  surface  coefficients  which  give  the  desired  behavior  system  on  the  sliding 
surface. 

F.  SLIDING  MODE  CONTROL  LAW 

Given  the  dynamic  model,  the  sliding  surface  definition,  and  the  stability 
criteria,  a  suitable  control  law  can  be  obtained.  We  assume  that  a  wide  range  of 
single  input,  single  output  dynamic  systems,  and  sliding  surfaces  ean  be  described 

by 


x  =  Ax+Bu 


(2.22) 


er(x,f)  =  STx(t) 

where  ST  is  a  row  vector  of  the  form  [l,  52,  53....jn_,],  a  specific  choice  of  ST  to 
achieve  a  stable  tracking  error  and  to  enhance  robustness  as  discussed  in  the 
previous  section.  If  m(/)  could  be  chosen  so  as  to  keep  the  trajectory  on 
c?(a-,/)  =  0,  we  would  have  the  sliding  control  law  from  the  sliding  condition 
equation  (2.14)  and  sliding  surface  equation  (2.16)  that 
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T*  o 

<r(.x)  =  5i  =  -rjsign(a) 


StA+StBu  =  - tilsign(c ) 

u  =  (2.23) 


where  t/g  is  a  arbitrary  nonlinear  feedback  gain 
The  control  law  has  two  parts. 


u  =  u+u 

where  u  is  linear  feedback  control  law 
u  is  nonlinear  feedback  control  law 

Initially,  ii  compensates  directly  for  the  known  portions  of  the  dynamics.  Thus, 
u  is  discontinuous  across  the  sliding  surface.  This  nonlinear  term  is  obtained  di¬ 
rectly  from  the  time-varying  bounds  on  parametric  uncertainty  and  disturbances. 
As  a  result,  control  discontinuity  across  (7=0  grows  as  the  model  becomes  less 
certain  and  increasingly  disturbed.  This  insures  that  a 2  is  a  Liapunov  function 
of  the  closed-loop  system,  since  it  satisfies  the  sliding  condition  equation  (2.14) 
and  thus  guarantees  stability  despite  the  uncertainty  in  the  model  and  disturb¬ 
ances  [Ref.  2].  This  type  of  control  law  can  guarantee  stability  and  perfect 
tracking  for  a  large  class  of  nonlinear  systems.  The  discontinuous  form  results  in 
a  chattering  type  of  control  action  that  would  be  very  undesirable  for  most  sys¬ 
tems  and  this  chattering  behavior  has  been  one  of  the  main  reasons  sliding  con¬ 
trol  techniques  have  not  been  more  widely  applied.  This  problem  will  be  solved 
by  smoothing  out  the  control  law  in  a  thin  boundary  layer  around  the  sliding 
surface  as  given  in  the  next  section. 
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G.  CONSIDERING  CHATTERING  AND  UNCERTAINTY 

While  sliding  mode  control  provides  a  control  law  which  is  robust  to  param¬ 
eter  variations  and  disturbance  inputs,  it  was  a  chattering  problem  for  the  input 
as  shown  in  Figure  5  on  page  17.  In  fact,  imperfections  such  as  delays  in 
switching  and  hysteresis  in  switching,  will  cause  the  trajectory  to  chatter  along  the 
sliding  surface.  Although  as  such  imperfections  vanish,  control  activity  remains 
as  undesirable  switching  and  high  frequency  signals  on  the  sliding  surface. 

The  basic  idea  of  eliminating  chattering  is  simple.  This  chattering  problem 
caused  by  a  discontinuous  and  nonlinear  switching  feedback  control  law  can  be 
eliminated  by  replacing  it  with  a  continuous  feedback  control  law.  But  if  the 
sliding  mode  control  has  a  continuous  feedback  function,  there  are  steady  state 
errors  due  to  variations  in  parameter  and  disturbance  [Ref.  2].  Suppose  the  con¬ 
trol  law  has  a  continuous  feedback,  whose  terms  are  continuous  function  inside 
a  small  boundary  layer  thickness  on  the  sliding  surface,  as  shown  in  Figure  6  on 
page  18,  then  the  steady  state  error  can  be  calculated  by  a  smooth  function  which 
eliminates  chattering  in  the  boundary  layer  thickness  (<£).  This  boundary  layer 
thickness  can  be  determined  directly  from  the  desired  sliding  surface  coefficient 
limit  and  estimates  of  the  uncertainty  dynamics  of  the  system  to  be  controller.  If 
the  specified  bounds  on  disturbances  and  parameter  uncertainty  are  not  ex¬ 
ceeded,  the  system  is  guaranteed  to  stay  within  the  boundary  layer  once  inside. 
If  a  disturbance  temporarily  exceeds  the  specified  bounds,  the  state  may  go  out¬ 
side  the  boundary  layer.  However,  the  sliding  condition  equation(2.14)  implies 
that  the  system  will  always  move  back  inside  the  boundary  layer  once  the  dis¬ 
turbances  return  to  their  projected  levels. 

The  dynamics  of  the  state  trajectory  inside  the  boundary  are  only  an  ap¬ 
proximation  to  the  desired  dynamics  on  the  sliding  surface.  The  advantage  of  the 
scheme  is  that  the  state  trajectory  does  not  chatter  close  to  the  sliding  surface. 
To  carry  out  the  preceding  program,  we  use  the  sliding  surface  considered  in  the 
previous  section  with  o-(av)  of  the  form: 

(7(a)  =  /,a  (r)  (2.24) 
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where  x(r)  =  *(/)-*,,(/) 

A',(r)  =  desired  state  variable 


To  define  the  boundary  layer  thickness  about  the  sliding  surface  of  equation 
(2.14),  define 

<7+(a)  =  o(x)+<p 

a~(x)  =  a(x)-<j>  (2.25) 

where  (f>  is  the  boundary  layer  thickness 

It  is  immediately  from  equation  (2.25)  that 
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Figure  6.  Boundary  layer  thickness  and  saturation  function 

1  <*(*)  1  ^  $ 

(2.26) 

and 

~  o~(x,t)  =  “  o{x,t)  =  ~  o+(x,t) 

(2.27) 

We  choose  the  control  law  u(t)  as  by  equation  (2.27) 

>  0.  This  guarantees  that 

for  c~(x,t)  <  0  or 

o 

A 

o 

1 

to 

(2.28) 

«+(*,0  <  0 

(2.29) 
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Equation  (2.28)  and  (2.29)  establish  that  trajectories  starting  outside  boundary 
layer  tend  towards  boundary  layer,  and  further  trajectories  starting  inside 
boundary  layer  stay  in  it  for  t0  <,  t .  It  only  remains  to  specify  u(x,t )  to  be  a  con¬ 
tinuous  function  of  x  inside  boundary  layer  thickness. 
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III.  SLIDING  MODE  CONTROL  FOR  NONLINEAR  A.U.V.  IN  DIVE 

PLANE 

A.  GENERAL 

Underwater  vehicles  present  difficult  control  system  design  problems  due  to 
their  nonlinear  dynamics,  uncertain  hydrodynamic  coefficients  and  the  presence 
of  disturbances  that  are  difficult  to  measure  or  estimate.  This  chapter  describes 
the  dynamics  of  a  selected  automonous  underwater  vehicle  (AUV)  and  the  design 
of  a  sliding  mode  controller  which  can  handle  these  problems  effectively. 

Motions  of  underwater  vehicles  are  expressed  in  a  body  fixed  reference  frame, 
because  hydrodynamic  forces  and  inertia  properties  are  most  readily  computed 
in  a  ship  reference  frame.  The  nonlinear  equations  of  motion  in  six  degrees  of 
freedom  vehicles  which  are  commonly  known  as  the  DTNSRDC  2510  equations 
of  motion  are  used  for  verification  of  the  sliding  mode  control  design.  These 
highly  nonlinear  equations  of  motion  are  linearized  by  a  Taylor  series  expansion 
and  modified  to  suit  the  needs  of  an  AUV  [Ref.  7].  First,  these  linearized  and 
Modified  equations  of  the  system  are  used  to  design  the  sliding  surface,  sliding 
mode  control  law  and  observer.  These  values  of  the  linear  system  are,  then,  used 
to  implement  the  sliding  mode  control  law  for  the  nonlinear  system.  This  chapter 
shows  how  to  design  the  sliding  mode  control  for  the  highly  nonlinear  AUV. 

B.  NONLINEAR  COMPUTER  MODEL 

The  nonlinear  model  used  for  sliding  mode  control  verification  was  derived 
from  the  original  NSRDC  2510  document  [Ref.  8].  The  nonlinear  model  used  in 
this  thesis  consists  of  8  differential  equations  which  describe  the  AUV  dynamics. 
The  six  equations  as  derived  from  force  and  moment  equalities  account  for  the 
states  u,  v,  w,  p,  q  and  r.  The  shape  of  AUV,  which  is  17.4  feet  long,  weighs 
12000  pounds,  and  neutrally  buoyant,  is  depicted  in  Figure  7  on  page  21. 
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General  dynamical  equations  are  derived  from  Newton's  law  in  an  inertial 
reference  frame: 

=j{  dynamical  response  terms)  (3.1) 

The  general  form  of  the  force  balance  is 

F  =  (Mx)  (3.2) 

where  M  =  mass  matrix 
X  =  [  U,  v,  w  Y 

and  the  moment  balance  is 

M=-jt(/Q)  (3.3) 

where  I  =  moment  of  inertia  matrix 

Q  =  0»  <h’'Y 

Three  dimensional  motions  of  underwater  vehicles  are  normally  described  using 
the  body-fixed  coordinate  and  inertial  reference  frame.  Position  of  the  body-fixed 
coordinate  system  is  expressed  in  X,  Y,  and  Z  coordinates  and  orientation  of  the 
vehicle's  coordinate  system  in  expressed  in  Euler  angles  6 ,  and  \p.  The  defi¬ 
nitions  of  u,  v,  w,  p,  q,  r  and  controls  are  listed  in  Table  1  and  Table  2. 


Table  1.  DEFINITION  OF  A  U  V  STATES. 


STATE 

DEFINITION 

UNITS 

u 

surge  rate 

(ft/s) 

V 

sway  rate 

(ft/s) 

w 

heave  rate 

(ft/s) 

p 

roll  rate 

(rad/'s) 

q 

pitch  rate 

(rad/'s) 

r 

yaw  rate 

(rad/’s) 

* 

spin 

(radians) 

e 

elevator 

(radians) 

«// 

azimuth 

(radians) 

Table  2.  DEFINITION  OF  A.U.V  CONTROLS 


CONTROL 

DEFINITION 

rudder  angle 

$bt 

starboard  bow  plane  angle 

^ bp 

port  bow  plane  angle 

<5, 

stern  plane  angle 

<>rm 

delta  form 

delta  buoyancy 

The  dynamical  response  terms  of  the  left  hand  side  of  equation  (3.1)  or 
equation  (3.2)  and  (3.3)  express  the  external  forces  and  moments  exerted  on  the 
vehicle  by  hydrodynamic,  control  surface,  propulsion  and  other  effects.  The  force 
and  moment  equalities  of  equation  (3.2)  and  (3.3)  describe  motions  in  six  degrees 
of  freedom  of  the  AUV.  The  three  forces  are  in  the  axial,  lateral  and  normal  di¬ 
rections  which  give  rise  to  motions  in  surge,  sway  and  heave  respectively.  The 
three  moment  equations  produce  moments  and  motions  in  roll,  pitch  and  yaw. 
Figure  8  on  page  24  shows  the  positive  directions  of  forces,  moments,  motions, 
and  control  surface  deflections. 
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The  equations  of  motion  for  the  six  degrees  of  freedom  for  the  fully  nonlinear 
model  are  listed  in  the  following  page.  The  hydrodynamics  coefficient  of  those 
equation  used  for  this  thesis  are  those  that  were  determind  using  an  analytic  ap¬ 
proach  [Ref.  7]  and  later  simplified  [Ref.  9].  The  four  nonlinear  equations  that 
are  considered  for  designing  sliding  mode  control  of  the  AUV  are  written  in  the 
following  form: 

Normal  Equation  of  Motion 


m[\v-uxq+vp+Xg{j)i—q)+  Yg{qr+p)-Zg{j)2+q2)']  = 
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f  ^[zy+zy’+z^+zv2] 


+  y  l3lZ'wv!+Z'qu^+Z'vvp+Z'„n '} 

+  j-i\z'^w+Z'„rWx(Z'iMZ'M 


p 

2 


[.CDyh(x)(y+xr)2+CD;,b(x)(w~xq)22 


[w-xg) 


+( !?''-£)  cos  0  cos  <£ 

+  y  /3zV*?£('/H  y  /2(z'vw,«*w+z,to,«^)6W) 


Pitch  Equation  of  Motion 


IyH(Ix-l2)P>‘-Iyz(pq->')+!X2(p2-r2)-nlXg{w-uxq+vp)~-Zg{u-vr+wq)l 
~  1~IM'  qq+M'  ppp2+M'prpr+M'nr2] 

+  Y  l\bl'  Ww+M'  quxq+M\pvp+M\rvr'] 

+ P[M\vuxw+M'  vy?2+ul(M’  fis5s-¥M'  6b5b)'] 


P_ 


[CDj/;(a-)(v+.xt)2+C0^(x)(u-'-av/)2] 


L!cj(x) 


xcix 


(3.4) 
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~(XgW-X ’„B)  cos  e  cos  <t>-{ZgW-Z„B)  sin  6 


+  j-  liM'nuMl)+ y  '*„«&)'«  (3.5) 

Kinematic  Relations 

0  =  q  cos  <f>~r  s\n  <j)  (3.6) 

Z  =  -ux  sin  6+v  cos  6  sin  </>+n>  cos  6  cos  (3.7) 

The  simulations  for  the  dive  plane  control  were  performed  by  using  the 
FORTRAN  language  code  (Appendix.  B)  for  the  simulation  of  nonlinear  system 
response  as  a  function  of  time. 

C.  LINEAR  MODELING 
1.  Equations  of  Motion 

The  sliding  mode  controller  design  procedure  begins  with  the  expression 
of  the  equations  of  motion  in  linear  time  invariant  state  space  form.  The  highly 
nonlinear  AUV  system  is 


(3.8) 

y{t)  =  g(*(0) 

(3.9) 

where  x  is  the  state  vector 

u  is  the  control  input  vector 
y  is  the  output  vector 

Although  equations  (3,8)  can  be  significantly  simplified  as  in  Larsen.[Ref.  9].  they 
appear  still  very  complex  for  this  study.  The  nonlinear  equations  can  be  linearized 
through  a  Taylor  scries  expansion  in  the  vicinity  of  a  nominal  point  (ideally  where 
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—  (0  =  0)  for  small  deviations  of  u(t)  and  x(t)  from  the  reference  values.  These 
equations  of  motion  were  linearized  by  Boncal  [Ref.  7].  Alternatively,  least 
squares  techniques  can  be  used  for  parameter  identification  in  order  to  develop 
a  linear  model  for  the  relationship  between  dive  plane  angle  (<5),  pitch  rate  (q), 
depth  (z)  and  pitch  angle  (6)  [Ref.  10].  The  linearized  and  very  simplified 
equations  for  the  dive  plane  motion 

q  =  -0.7^—0.030—0.035(5 

0  =  q 


z  =  -U06  (3.10) 

were  found  to  provide  a  satisfactory  approximation  of  the  open  loop  dive  plane 
dynamics  of  nonlinear  equation  (3.4)  to  (3.7)  for  the  nominal  speed  of  6  ft/'sec  (or 
500  rpm ). 

2.  Sliding  Surface  Design 

It  is  evident  from  the  discussion  of  the  sliding  mode  theory  (Chapter  2) 
that  description  of  AUV  motion  depends  on  the  sliding  surface  regardless  of  dis¬ 
turbance  and  unmodel  parameters,  after  it  hit  sliding  surface.  So  it  is  very  im¬ 
portant  to  design  sliding  surface  of  the  AUV  with  disturbance  and  unmodelled 
parameters.  The  sliding  surface  of  the  AUV  will  be  designed  based  on  the  linear 
model.  The  state  space  form  of  the  linear  model  is 

x  =  Ax+Bu 

y  =  Cx  (3.11) 


where  xT  =  [<7, 6,  z] 


A  = 


-0.7 

1 

0 


-0.03  0 
0  0 
-Vo  0 
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B  = 


-0.035 

0 

0 


and 


u  =  S 

C=  [00  1] 


For  a  three  dimensional  system,  the  sliding  surface  is  the  Euclidean  plane 
<r(x)  =  Srx  =  0 


where  5  = 


Equation  (2.6)  can  be  expressed  in  terms  of  the  state  variables  and  sliding  surface 
coefficients  as  following 

<t(x)  =  q+s20+s3z  (3.12) 


where  the  coefficient  of  q  has  been  normalized  to  1.  S  will  determine  the  sliding 
surface  plane  uniquely.  To  compute  the  equivalent  control  ( ueq ),  we  substitute  A, 
B  and  S  in  to  equation  (3.11),  and  the  closed-loop  dynamics  of  the  linear  model 
are 


a-  =  [/i-^S7^)-^7^]* 


or 


x  =  (A-BK)x 


(3.13) 
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where  the  gain  vector  K  can  be  found  from  standard  pole  placement  methods. 
The  closed-loop  dynamics  matrix 


Ac  =  A-BK  (3.14) 

where  K  =  {STB)~'STA 

has  eigenvalues  specified  for  desirable  response  of  the  AUV.  One  of  the 
eigenvalues  of  Ac  must  be  specified  as  zero.  With  Ac  specified  and  K  computed 
from  pole  placement,  ST  matrix  can  be  determined  as  using  the  equation  (3.13) 
and  (3.14). 

St(A—BK)=0 


or 

STAc  =  0  (3.15) 

Therefore  ST  is  found  as  a  left  annihilator  of  Ac  or  S  is  a  left  eigenvector  of  Ac 
which  corresponds  to  the  zero  value.  This  sliding  surface  of  the  AUV  satisfy  the 
sliding  surface  condition  Srx  =  0.  To  find  proper  sliding  surface  of  the  AUV, 
Matrix-x  program  "SCM"  in  Appendix  A  is  used.  The  response  of  the  AUV  ac¬ 
cording  to  different  sliding  surface  is  shown  in  Figure  9  on  page  30,  Figure  10 
on  page  31,  and  Figure  11  on  page  32.  The  sliding  surface  of  Figure  10  which 
has  no  overshot  and  fast  response  time  is  selected  as  the  reference  sliding  surface 
of  the  AUV  in  order  to  design  control  law.  The  sliding  surface  of  the  AUV  is 

a{x)  =  <7+0.520—0.01 12z  (3.16). 

This  sliding  surface  has  the  desired  dynamics  of  the  closed-loop  system.  The 
perfect  depth  tracking  of  the  selected  AUV  is  then  defined  as  remaining  along  the 
surface.  The  dynamic  response  of  AUV  is  affected  by  the  chosen  sliding  surface. 
The  selected  sliding  surface  is  used  to  handle  accurate  depth  control. 
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(RfiD/SEC)  DEPTH (FT) 
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(RRO/SEC)  DEPTH (FT) 
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3.  Sliding  Mode  Control  Law 

Here  the  goal  is  to  determine  switched  feedback  gains  which  will  drive  the 
AUV  state  trajectory  to  the  sliding  surface  and  maintain  sliding  mode  conditions. 
By  defining  the  Liapunov  function  of  the  equation  (2.13) 

m-  yOM]2  (3.17) 

where  a(x)  =  <7+0.520-0.01 12z 

asymptotic  stability  of  the  AUV  on  the  sliding  surface  is  guaranteed,  that  pro¬ 
vided  F(x)  is  a  negative  definite  function  or 

=  -V I  *(*)  I 

or 

o-(.v)  =  -i]2sign(c(x))  (3.18) 

Since  o-(a-)  =  STx,  we  have 

5r(^A*+5w)  =  -}]2sign(o(x))  (3.19) 

and  solving  for  the  equivalent  control  input  u 

u  =  -{STB)~lSTAx-{STB)~lt]2sign(d)  (3.20) 

It  is  important  to  recognize  that  the  feedback  control  law  is  composed  of  two 
parts, 

u  =  u+u 

The  first  u  =  ~(STB)~]STAx  is  a  linear  feedback  control  law,  where  the  second 
u  -  -(ST~B)-]}j2sign(a(x))  is  a  nonlinear  feedback  with  its  sign  toggling  between 
plus  and  minus  according  to  which  side  of  the  sliding  plane  the  AUV  is  located 
in.  Two  comments  arc  in  order  here:  First,  since  u  has  to  change  its  sian  as  the 


AUV  crosses  a{x)  =  0,  the  sliding  surface  has  to  be  a  hyperplane  (dimension  of 
one  less  than  the  state  variable).  Second,  it  is  17  which  is  mainly  responsible  for 
driving  and  keeping  the  AUV  onto  the  sliding  plane  a(x)  =  0  (where  u  =  0  as 
well).  The  block  diagram  of  the  sliding  mode  control  is  illustrated  in  Figure  12 
on  page  34. 


Figure  12.  The  block  diagram  of  the  sliding  mode  control 


Provided  that  the  gain  has  been  chosen  large  enough,  u  can  provide  the 
required  robustness  due  to  momentary  disturbance  and  unmodcled  AUV  without 
any  compromise  in  stability.  The  linear  feedback  law  is  designed  such  that  the 
AUV  has  the  desired  dynamics  on  the  sliding  plane.  Since  (t(,y)  =  0  in  this  case 
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u  =  u  =  ~{STB)~lSTAx 


(3.21) 


Large  enough  gain  (E7A-t]2{STB)-1  =4.0)  could  be  used  in  the  computer  simu¬ 
lation  by  using  Matrix-x  and  FORTRAN  program  "SMC"  in  Appendix  A.  The 
dynamic  response  of  the  AUV  according  to  several  values  of  gain  is  shown  in 
Figure  13  on  page  37,  Figure  14  on  page  38,  and  Figure  15  on  page  39.  Choos¬ 
ing  properly  large  enough  nonlinear  feedback  gain,  although  the  response  time 
of  the  AUV  is  fast,  the  AUV  overshoots  on  the  sliding  surface  as  shown  in 
Figure  1 5  on  page  39.  The  dive  plane  angle  of  the  AUV  presents  chattering  due 
to  sign  switching  of  the  nonlinear  part  of  the  control  law.  Several  varieties  of 
nonlinear  feedback  gain  (t]2)  and  boundary  layer  (<£)  are  evaluated  through  a  se¬ 
ries  of  numerical  experiments  in  order  to  analysia  dynamic  response  of  AUV 
which  are  dependant  on  it.  The  characteristic  of  dynamic  response  according  to 
the  nonlinear  feedback  gain,  boundary  layer,  and  closed-loop  poles  are  shown  in 
Table  3. 


Table  3.  THE  DYNAMIC  CHARACTERISTIC  OF  THE  AUV  ACCORDING 


TO  DESIGN  PARAMETER  s 


Parameters 

Depth  re¬ 
sponse 

Dive  plane 
angle 

Pitch  angle 
(radians) 

Sliding  sur¬ 
face 

C.P  = 

0, ‘6.1. -0.15 

slow,  no 
overshoot 

no  saturated 

maxi 

angle  =  0.4 

no  overshoot 

C.P  = 

0,-0.25,'0.27 

fast,  no  over¬ 
shoot 

no  saturated 

maxi 

angle  =  0.41 

no  overshoot 

C.P  = 

0, -0.45, -6.47 

fast,  over¬ 
shoot 

saturated 

maxi 

angle  =  0.42 

overshoot 

ETA  =  3. 
PHI  =0.2 

slow,  over¬ 
shoot 

no  saturated 

maxi 

angle  =  0.4 

overshoot 

ETA  =  4, 
PH!  =0.4 

fast,  no  over¬ 
shoot 

no  saturated 

maxi 

angle  =  0.4 1 

no  overshoot 

ETA  =  8. 
PHI  =0.4 

fast,  no  over¬ 
shoot 

saturated 

maxi 

angle  =  0.42 

overshoot 

We  already  knew  that  system  behavior  is  dependent  on  the  sliding  surface  which 
is  designed  based  on  closed-loop  poles.  The  closed-loop  poles  (0, -0.25, -0.27), 
nonlinear  feedback  gain  (ETA =4),  and  the  boundary  layer  (0=0.4)  are  selected 
in  order  to  satisfy  the  necessary  motions  of  the  AUV. 


36 


DEPTH (FT) 


38 


ANGLE ( RAD /SEC )  DEPTH ( FT ) 


0.0  30.0  60.0  90.0  120 

TIME (SEC) 


0.0  30.0  60.0  90.0  120 

TIME (SEC) 


TIME (SEC)  TIME (SEC) 


Figure  15.  The  dynamic  response  for  ETA(8)  and  PHlfO.4) 


4.  Chattering  Problem  and  Steady  State  Error 

The  nonlinear  part  of  the  sliding  mode  control  may  give  rise  to  chattering 
where  constant  or  random  disturbances  are  present,  as  shown  in  Figure  16. 


TIME (SEC )  TIME ( SEC ) 


Figure  16.  The  chattering  problem  of  sliding  mode  control 

A  choice  of  a  saturation  function  (satsgn(a))  instead  of  the  pure  switch  (sign(cr)) 
for  the  nonlinear  control  law  is  prcfered  in  order  to  avoid  chattel ing  problems. 
Since  the  boundary  laj'er  thickness  is  related  to  the  characteristic  of  dynamic  re¬ 
sponse,  it  can  be  selected  according  to  the  manuvering  conditions  of  underwater 
vehicles.  Therefore  the  conirol  law  for  the  AUV  is  modified  to 

u=  -5A429q+l.07\40+Knsatsgn((r)  (3.22) 

where  K„  =i]2{STD)-' 


and 
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+  1  if  0^0 

=  -1  if  (7  <,  -0 

l/  -0  <  0  <  0 

This  provides  a  linear  interpolation  across  the  boundary  layer  as  illustrated  in 
Figure  17  on  page  42. 

A  boundary  layer  thickness  (0)  essentially  assigns  a  lowpass  filter  structure  to  the 
dynamics  of  the  sliding  surface  (0) .  If  a  specific  bound  on  disturbance  is  not  ex¬ 
ceeded,  the  system  is  guaranteed  to  stay  within  the  boundary  layer  once  inside. 
The  steady  state  error  of  the  sliding  mode  control  of  the  AUV  with  a  disturbance 
can  be  computed  as  following.  From  the  equation  (3.11) 

q  =  -0.7tf— 0.030— 0.035m+<*  (3.24) 

where  d  is  disturbance 
q  =  <7  =  0 

0  =  0  in  steady  state 


The  steady  state  control  input  for  the  given  disturbance  is 

f  ,  d 
'ss  0.035 


(3.25) 


The  control  law  in  steady  state  is 

u  \ss  =  u+u  =  q2satsgn(  ~ )  (3.26) 


where  0(x)  = -0.01 12c, 

^Z  2  4 c 
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Figure  17.  The  saturation  function  for  the  nonlinear  control  law. 


2  -0.01 12  x  e,  4 

J — L) 


«  L  X  </> 
0.0112  x>/2 


(3.27) 


or 


_ dx  (j) _ 

0.01 12  x?/2x  0.035 


(3.28) 


The  steady  state  error  can  be  minimized  by  increasing  the  nonlinear  control  gain 
(;/:)  and  decreasing  boundary  layer  {<f))  as  shown  in  Figure  18  on  page  44.  How¬ 
ever,  it  can  never  be  completely  eliminated.  The  depth  response  of  the  Figure  18 
(a)  show  the  steady  state  error  for  the  constant  disturbance  (d  =0.005)  when  us¬ 
ing  the  nonlinear  feedback  gain  (ETA  =  4).  Large  nonlinear  feedback  gain  is  used 
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in  order  to  decrease  the  steady  state  error  as  shown  in  Figure  IB  (b).  The  steady 
state  error  for  the  command  depth  is  decreased  down  to  10%  approximately  us* 
ing  large  gain.  However,  dive  plane  is  kept  the  same  angle  regardless  nonlinear 
feedback  gain.  If  we  use  large  gain  and  small  boundary  layer,  the  depth  response 
of  the  AUV  may  overshoot  and  the  dive  plane  input  may  have  numerical  chat¬ 
tering.  This  is  different  from  the  chattering  due  to  disturbance  and  unmodeled 
dynamics  as  shown  in  Figure  19  on  page  45.  This  numerical  chattering  problem 
is  an  artifact  of  the  numerical  integration  method  (a  fixed  step  Kutta-Merson  and 
fixed  Euler's  method  were  used  in  the  simulation)  and,  in  principle,  it  can  be  re¬ 
duced  by  choosing  a  more  accurate  method  with  smaller  time  step. 
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5.  Robustness  tests 

Another  important  aspect  of  the  sliding  mode  control  is  its  robustness  to 
parameter  variations  and  unmodeled  dynamics.  Sliding  mode  controllers  are  ex¬ 
pected  to  outperform  more  conventional  linear  state  feedback  with  respect  to 
robustness.  This  is  true  for  the  following  two  reasons:  First,  choosing  r f  suffi¬ 
ciently  large  will  guarantee  that  the  system  approaches  the  sliding  surface 
asymptotically  even  for  the  off-design  case.  Second,  since  at  the  final  stage  the 
system  evolves  in  a  lower  dimension  state  space  (the  sliding  surface),  it  is  na¬ 
turally  more  robust  than  the  original  higher  dimensionality  system.  To  verify 
these  expectations,  we  changed  the  hydrodynamic  coefficient  of  the  AUV,  AUV 
rpm  and  dive  plane  strength  as  in  Table  4.  The  standard  design  AUV  equation 
are  designated  by  A,  B  matrices  at  6  ft/'sec  (rpm  =  500). 


Table  4.  ROBUSTNESS  TEST  CASES 


TEST.  NO 

A  MATRIX 

B  MATRIX 

SPEED(RPM) 

Nominal  test 

A 

B 

6  ft/sec  (500) 

Test  1 

2*  A 

B 

6  ft/'sec  (500) 

Test  2 

A 

2*B 

6ft/sec  (500) 

Test  3 

A -2 

B 

6  ft/sec  (500) 

Test  4 

A 

B/2 

6  ft/sec  (500) 

Test  5 

2*  A 

2*B 

6  ft/sec  (500) 

Test  6 

A/2 

B/2 

6  ft/sec  (500) 

Test  7 

A 

B 

12  ft/sec  (1000) 

The  sliding  mode  control  for  the  AUV  was  designed  based  on  the  nominal  case 
as  in  the  previous  section.  The  nominal  nonlinear  feedback  gain  (?/2  =  2.4)  and 
boundary  layer  thickness  ($  =  0.4)  were  applied  to  the  modified  system.  The  dy¬ 
namic  response  of  the  modified  AUV  is  shown  in  Figures  14,  and  20,  to  26.  The 
A  matrix  of  test  1  model,  which  is  related  to  the  rotary  damping  coefficient,  was 
doubled  in  magnitude  in  order  to  test  the  sliding  mode  control.  Only  the  response 
time  of  test  1  model  was  longer.  The  test  3  model,  which  has  a  matrix  equal  to 
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one-half  of  the  A  matrix,  presents  fast  response  and  overshoot  due  to  decrease  in 
the  the  rotary  damping  moment  as  shown  in  Figure  22.  The  hydrodynamic  coef¬ 
ficient  for  the  dive  plane,  which  in  proportional  to  dynamic  response  time,  was 
changed  in  order  to  test  performance  of  variable  structure  system  for  the  AUV. 
In  general,  it  is  evident  from  the  simulation  results,  that  even  under  a  200% 
change  in  the  coefficient  of  the  A,  B  and  RPM,  the  vehicle  response  remains 
stable,  7  ~  8  %  overshoot,  faster  or  slower  as  expected. 
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D.  DESIGN  OF  SLIDING  MODE  CONTROL  FOR  THE 
NONLINEAR  MODEL 

The  sliding  mode  appeal  is  based  on  its  ability  to  treat  nonlinear,  time  varying 
and  unmodelled  systems  in  a  straightforward  manner  [Ref.  3].  It  is  the  purpose 
of  this  section  to  apply  the  linear  sliding  mode  controller  to  the  nonlinear  model 
of  the  actual  AUV.  The  sliding  surface  and  control  law  gain  for  the  actual  AUV 
are  based  on  the  linearized  nominal  model.  The  mathematical  form  of  the  non¬ 
linear  model  for  the  actual  AUV  is 

=/!jc+A/[.r)+B(x)w+4jc,  t)  (3.29) 

where  B(x)  is  the  nonlinear  function  associated  with  the  control 
surface  and  actuator  system 

A/lx)  is  the  uncertainty  of  the  nonlinear  function  f(x). 
d{x,  t)  is  the  uncertainty  disturbance. 

The  system  matrix  A  is  the  estimate  of  f(x)  and  the  magnitude  of  the  uncertainty 
is  bounded  as 


F  S  |  SrAJ{x)  | 


(3.30) 


where  F 


The  individual  bounds  on  any  element  of  Af  as  estimated  from  some  knowledge 
of  the  extremes  of  possibility  of  Aj\x).  Also,  let  B(x)  be  approximated  by  B,  a 
constant,  where  the  varying  gain  ft  is  defined  by  B  =  pB(x)t  and  /?  is  taken  to  be 
scalars,  but  bounded  within  the  limits  of  (lmn  <  ft  <  /?max,  and  =  1 .  Then  it 
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follows  that  p  =  1,  and  Af  =  0,  and  d(x,t)=0  will  yield  the  nominal  sliding  mode 
control  law  (3.20).  Disturbance  d(x,t)  is  unknown  but  is  upper  bounded  by  a 
known  continuous  function  such  that: 


D  >  |  SV(x,f)| 


(3.31) 


The  dynamics  of  the  system  with  bounded  uncertainty  of  the  actual  AUV  is 


=  Ax+AJ[x)+PBu+d(x ,  r) 


(3.32) 


This  equations  is  used  to  simulate  the  uncertain  nonlinear  terms  after  the 
linearization  process.  We  have  designed  the  sliding  surface  cr(x)  =  STx  by  using 
the  nominal  linearized  model  in  the  previous  section.  Using  the  sliding  condition 
theory  for  the  actual  AUV,  the  sliding  mode  control  law  for  the  nonlinear  model 
can  be  chosen  from  equations  (3.17)  and  (3.18)  such  that  if 

<r(.Y)<7(.v)  =  -?/2  |  <r(x)  i 

and  the  system  will  reach  the  surface  (e?(x)  =  0)  within  a  finite  time  tr  defined  by 


(3.33) 


The  true  dynamics  of  the  sliding  surface  with  uncertainty  are,  however,  given  by 


a(x)  =  STx 


—  St[Ax+AJ{x)+P Bu+d(x,  /)] 


(3.34) 


Substituting  equation  (3.20)  to  the  above  equation  for  u,  then  the  derivatives  of 


<7  =  (\-rl)STAx+STAJ{x)+STd(x,  t ) 


-P  l>fsign{o) 


(3.35) 
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From  the  above,  stability  is  guaranteed,  if  and  only  if 

c(x)  £  -tilsign{<r),  (3.36) 

where  til  is  the  nonlinear  feedback  gain  without  an  uncertainty 

If  the  control  matrix  B  is  exactly  known  (0  =  1),  then  control  law 

ub  =  (3.37) 

where  t/2  ^  |  ti20+F(x)+D(x,  0  I 

can  guarantee  stability  and  perfect  tracking  for  the  nonlinear  system  with  con¬ 
stant  control  matrix  of  the  AUV.  In  case  where  the  control  system  B(x)  is  un¬ 
certain,  the  following  change  (tj2)  must  be  made: 

u  =  -[STBJ~lSTAx-iSTBY]ri2signa(x)  (3.38) 

u  =  u+u 

where  u  =  -lSTB}-]STAx 

u  -  -lSTByiti2sign{c) 

>12  *  /U  I  ri+F(x)+D(x,  t)  I  +  I  (pmiX- 1)  1 1  S* Ax  I 

Since  the  nonlinear  term  of  the  B(x)  and  A/(x)  is  uncertain  in  most  cases,  it  is 
assumed  to  be  zero  equation  (3.29)  and  t\2  is  increased  depending  on  their  as¬ 
sumed  bonuds  to  guarantee  stable  sliding  mode  control  [Ref.  2].  The  actual 
control  law  used  in  subsequent  simulation  was  in  fact  equation  (3.38)  with  suffi¬ 
ciently  large  ij2  to  accomodate  the  uncertainty  in  AJ\x)  and  B(x).  The  sliding 
mode  switching  control  law  equation  (3.38)  guarantees  that  equation  (3.18)  is 
satisfied  even  in  the  presence  of  parameter  variations  and  unmodeled  dynamics 
provided  is  large  enough.  The  dive  plane  angle  chattering  due  to  modeling  er¬ 
rors  and  disturbances  can  be  eliminated  by  defining  a  boundary  layer  thickness 
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(j)  about  a  =  0  as  illustrated  in  the  previous  subsection.  The  smooth  sliding  mode 
control  law  of  the  actual  AUV  is  then 

j))  (3-39) 

where  t]1  ^  I  r\l+F{x)+D{x,  t)  |  +  |  (/?max- 1)  |  j  STAx  | 

The  dynamic  response,  dive  plane  angle,  and  sliding  surface  obtained  from  non¬ 
linear  model  simulation  at  500  rpm  are  shown  in  Figure  27  on  page  59.  Al¬ 
though  the  sliding  surface  and  control  law  gain  based  on  the  nominal  linear 
system  were  applied  to  the  nonlinear  system,  the  response  of  the  system  is  satis¬ 
factory  as  expected.  The  designed  variable  structure  system  based  on  the  nominal 
linear  equations  dealt  with  the  full  nonlinear  dynamics  of  the  AUV  as  shown  in 
Figure  27.  The  technique  of  sliding  mode  control  can  handle  the  nonlinear  sys¬ 
tem  directly  without  linearization,  if  sliding  surface  coefficients  are  properly  cho¬ 
sen.  This  is  especially  important  for  highly  maneuverable  underwater  vehicles 
that  can  move  in  all  directions. 


u  —  —  [S7#]  !( STAx+tj2satsign( 
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TIME (SEC)  TIME (SEC) 


Figure  27.  Tlte  response  of  the  actual  AUV 
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E.  ROBUSTNESS  TESTS  USING  THE  NONLINEAR  MODEL 

The  variable  structure  systems  have  an  important  property  which  is  much  less 
sensitive  to  nonlinearities,  disturbances,  and  unmodeled  dynamics  as  illustrated 
in  the  previous  chapter.  The  nonlinear  model  of  the  AUV  is  used  to  demonstrate 
the  properties  of  this  control  methodology.  Accurate  models  are  normally  re¬ 
quired  in  order  to  achieve  good  control.  However,  since  hydrodynamic  modeling 
is  a  key  element  in  the  design  of  control  system  for  AUV,  we  have  to  rely  on  ap¬ 
proximate  expressions  for  the  hydrodynamic  forces  which  can  introduce  large  er¬ 
rors  into  the  control  process.  We  have  already  seen  the  results  of  simulation  for 
control  performance  using  an  accurate  linear  model,  modified  linear  model  and 
accurate  nonlinear  model.  In  this  section,  a  simulation  study  is  performed  to  il¬ 
lustrate  the  effectiveness  of  the  sliding  control  under  large  modifications  in  the 
nonlinear  model  parameters.  The  modified  hydrodynamic  coefficients  in  Table 
4  are  closely  related  to  system  stability.  The  rotary  damping  coefficient  (Mq)  af¬ 
fects  the  hydrodynamic  moment  of  the  AUV  in  the  vertical  plane.  The  center  of 
gravity/center  of  buoyancy  separation  is  directly  proportional  to  restoring  mo¬ 
ment.  Hydrodynamic  coefficient  (MSj)  directly  affects  the  pitch  moment  gener¬ 
ated  by  the  dive  planes.  Speed  was  changed  from  500  rpm  to  1000  rpm. 


Table  5.  TEST  CASES  FOR  THE  NONLINEAR  MODEL 


TEST  NO 

K 

Z* 

A4 

RPM 

Test  1 

2*Mq 

Z * 

M* 

500 

Test  2 

2  *M, 

Z„  !2 

2*iW* 

500 

Test  3 

K 

z* 

2*Mgs 

500 

Test  4 

■H 

Zx  [2 

M*  12 

500 

Test  5 

K . 

z, 

A4  a 

500 

Test  6 

K 

ZK  '4 

Mdt 

500 

Test  7 

Zg 

1000 

The  controller  used  was  based  on  the  nominal  linear  model,  while  some  vehicle 
coefficients  differed  by  200%  and  nonlinear  terms  were  added.  The  FORTRAN 
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program  in  Appendix  B  was  used  for  this  purpose.  Time  plots  of  depth,  pitch 
rate,  sliding  surface,  and  pitch  angle  response  along  with  the  depth  commanded 
values  are  shown  from  Figure  28  on  page  62  to  Figure  33  on  page  67. 

The  sliding  mode  controller  is  shown  to  provide  stable  response  and  performs 
consistently  at  different  parameter  values  and  speed.  In  every  case,  the  system 
remains  inside  the  boundary  layer  (a  =  0).  It  is  felt  that  the  sliding  mode  method 
can  produce  extremely  robust  controllers  that  perform  predictably  despite  the  use 
of  simplified  or  unmodeled  dynamics.  The  center  of  gravity  of  the  AUV  is  the 
most  sensitive  parameter  in  the  vehicle  dynamic  response.  The  depth  response 
of  the  AUV  did  not  overshoot,  although  the  center  of  gravity  was  close  to  the 
center  of  buoyancy.  In  general,  the  rpm  affected  the  dynamic  response  of  the 
nonlinear  vehicle.  In  this  case,  although  the  rpm  was  doubled  in  magnitude,  the 
overshoot  was  not  present  and  the  response  time  reduced  to  35  seconds.  When 
variation  of  parameters  and  modeling  error  of  the  AUV  were  increased,  the 
robustness  of  the  sliding  control  was  improved  by  increasing  the  nonlinear  feed¬ 
back  gain  (if).  Of  course,  if  the  uncertain  values  exceed  the  limited  boundary, 
the  sliding  mode  control  will  not  handle  these  variations  effectively.  The  chosen 
nonlinear  feedback  gain  (if  =  2.4)  for  the  selected  AUV  was  enough  to  handle  the 
nonlinear  terms  and  the  modified  hydodynamic  coefficients.  The  modeling  er¬ 
rors,  variations  of  parameters,  and  nonlinear  terms,  which  were  difficult  problems 
of  robust  using  other  control  technique,  were  easily  dealt  with  using  the  sliding 
mode  control  as  shown  from  the  results  of  simulations. 
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Figure  28.  The  dynamic  response  of  test  1  AUV  (depth  command  =  100  ft) 
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Figure  29.  The  dynamic  response  of  test  2  AUV  (deptli  command  =  100  ft) 
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Figure  30.  The  dynamic  response  of  test  3  AUV  (depth  command  =  100  ft) 
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Figure  31.  The  dynamic  response  of  test  4  AUV  (depth  connnnnd=  100  ft) 
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Figure  32.  The  dynamic  response  of  test  5  AUV  (depth  command  =  100  f() 
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Figure  34.  The  dynamic  response  of  test  7  AUV  (rpm=  1000) 
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IV.  DESIGN  AND  EVALUATION  OF  A  F  'DING  MODE 

COMPENSATOR 


A.  GENERAL 

An  autonomous  underwater  vehicle  (AUV)  must  operate  under  its  own  power 
and  be  capable  of  navigation  and  guidance  with  sufficient  accuracy  to  be  easily 
recoverable.  A  navigation  system  for  an  underwater  vehicle  is  subject  to  vehicle 
size  or  cost  restrictions  and  this  limits  the  ability  to  install  the  highly  accurate 
sensors  needed  to  produce  reliable  pitch  rate  data.  The  vehicle  depth  can  be 
measured  directly  by  a  pressure  cell  sensor  aboard  the  vehicle.  Using  the  depth 
of  the  vehicle  as  the  only  external  input,  state  observers  can  provide  all  remaining 
controller  data.  It  is  desirable  to  investigate  the  performance  of  a  sliding  mode 
compensator  designed  for  a  linear  model  and  applicable  to  a  nonlinear  model 
with  unmodeled  vehicle  dynamics.  It  is  the  purpose  of  this  chapter  to  design 
such  a  sliding  mode  compensator  and  assess  its  robustness.  This  work  involves 
numerical  simulation  of  the  performance  of  a  sliding  mode  compensator  scheme 
using  a  linear  and  a  full  12  state  nonlinear  model  of  the  equation  of  motion  for 
the  AUV. 

B.  DESIGN  OF  SLIDING  MODE  COMPENSATOR 
1.  Linear  Model 

A  state  observer  is  designed  which  uses  measured  depth  only  in  order  to 
estimate  the  vehicle  pitch  rate  and  pitch  angle.  The  block  diagram  of  the  basic 
sliding  mode  compensator  is  illustrated  in  Figure  35  on  page  71.  The  state  ob¬ 
server  design  is  based  on  the  equation  [Ref.  1 1]. 

x  =  Ax+Bu+Kg()>-Cx)  (4.1) 

where  x  is  the  state  variable  estimated  by  the  observer,  and  K0 
is  the  observer  feedback  which  is  required  for  the  observer  to  be  able  to  follow 
and  duplicate  control  system  operation. 
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A,  B,  and  C  are  the  original  open-loop  control  matrices  of 

equation  (3.11). 

u  is  the  sliding  mode  control  law  based  on  the  observer. 

Note  that  in  the  sliding  mode  compensator,  disturbance,  nonlinear  terms,  and 
variation  of  parameters  are  ignored.  Equation  (4.1)  is  schematically  depicted  in 
Appendix  A.  Collecting  terms  and  rearranging 

x  =  {A-K0C)x+Bu+Kay  (4.2) 

which  is  then  rearranged  into  the  familiar  state  space  form: 

A  >  ,  D 
*  =  Ax+Bu 

y  =  Cx+DU  (4.3) 

where  A  is  the  observer  A  matrix  (A-KqC) 

B  is  the  observer  B  matrix 
C  is  the  observer  C  matrix 

The  values  for  observer  feedback  gain  ( K0 )  are  calculated  using  the  Matrix-x 
program  "OBSERVER"  in  Appendix  A.  This  provids  three  values  of  gain  for  the 
one  state  (depth)  that  is  used  by  the  observer  in  order  to  estimate  the  remaining 
two  (pitch  rate  and  pitch  angle).  The  poles  chosen  for  the  observer  are  -4.5,  -4.75, 
and  -4.95.  These  values  are  selected  because  they  create  a  faster  response  in  the 
observer  than  in  the  controller  itself.  This  condition  is  necessary  to  ensure  that 
the  observer  will  not  slow  down  the  overall  simulation  speed  of  the  controller. 
The  sliding  surface  of  the  sliding  mode  compensator  has  the  same  form  as  the 
sliding  surface  of  a  controller  c(x)  =  STx. 

The  state  obsener  provides  necessary  information  to  the  sliding  mode  controller. 
The  control  input  of  the  sliding  mode  compensator  can  be  described  in  the  fol¬ 
lowing  process.  From  the  sliding  condition  we  have 
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7! 


a(x)  =  - n2sign{a ) 


(4.4) 


Substituting  equation  (4.3)  to  equation  (4.4)  and  rearranging 

STx  =  - r\2sign{a ) 

St(Ax+Bu)  =  —t]2sign(a)  (4.5) 

Finally,  the  sliding  mode  control  law  can  be  expressed  in  terms  of  the  estimated 
state  variables  in  the  following  form: 

u  =  ~{STB)-lSTAx-(STB)-li12sign(G)  (4.6) 

or 

u  =  u+u 

where  u  =  -(Sr5)“!S7^.v 

u  =  -(STB)~])}2sign(o) 

The  Figure  36  shows  the  dynamic  response  of  the  vehicle  with  the  sliding  mode 
compensator.  The  nonlinear  feedback  gain  (?/2)  in  the  compensator  is  the  same 
as  the  gain  in  the  controller  without  the  observer.  It  can  be  seen  that  since  the 
observer  poles  are  fast,  the  performance  of  the  current  compensator  resemble  that 
of  the  controller  alone  of  Chapter  3.  A  sliding  mode  compensator  is  easier  to 
design  and  implement  than  conventional  compensators. 


PITCH  ANGLE  (RAP/'CLC)  DEPTH (FT) 

-0.7  -0.S  -0.3  -0.1  0.1  0.3  -5.0  20.0  45.0  70.0  95.0  120.0 
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2.  Nonlinear  Model 

The  designed  nonlinear  control  feedback  gain  Of2),  sliding  surface  (<r(x)), 
and  observer  feedback  gain  (K0),  which  are  based  on  the  nominal  linear  model, 
are  used  to  design  a  sliding  mode  compensator  in  order  to  handle  a  full  nonlinear 
model.  Since  a  nonlinear  model  can  never  be  as  easily  predictable  as  a  linear 
model,  there  exists  certain  degree  of  uncertainty  in  the  nonlinear  terms.  A  similar 
process  as  in  Chapter  3  provides  a  design  of  a  nonlinear  sliding  mode 
compensator.  The  AUV  nonlinear  dynamics  is  given  by 

x  =  Ax+AJ[x)+B(x)u  (4.7) 

where  A f(x) ,  B(x)  are  the  model  errors  of  nonlinear  terms  and 

uncertainty. 

Equation  (4.7)  can  be  rearranged  by  using  the  same  proceedure  in  Chapter  3. 

x  =  Ax+AJ{x)+(iBu  (4.8) 

The  procedure  for  obtaining  „v(/)  of  x(t)  is  to  compute  the  estimate  to  be  the  out¬ 
put  of  the  dynamic  system. 


x  =  Ax+AJ\x)+PBu+K0(y-Cx)  (4.9) 

We  can  rewrite  (4.9)  in  the  following  state  space  form 


x  =  Ax+A/[x)+(iBu 

y  =  Cxu  (4.10) 

where  F{x)  >  \  STAJ{x)  | 
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The  closed-loop  dynamics  of  the  state  observer  on  the  sliding  surface  with  un¬ 
certainty  is  given  by 


cr(Jc)  =  STx 

=  STtAx+Af{x)+(}Bu]  (4.11) 

Substituting  equation  (4.6)  to  the  above  (4.1 1)  for  u,  then  we  obtain  the  following 
equation 

<7  =  (l—p~1)STAx+STAJ[x) 

-p~li]2sign<j{x)  (4.12) 

If  c  <  -ijlsigna(x)  is  enforced,  stability  will  result.  We  can  establish  conditions 
on  the  use  of  ?/2  in  equation  (4.6)  that  will  guarantee  that  sliding  condition,  given 
the  bounds  of  uncertainty.  It  follows  that 

H2  2  P0io+STAAx)+(l  -r')STAx)  (4.13) 

which  can  be  achieved  by 

r  S  ft™  |  >^I-F(*)  I  +  I  (ft„„-l)  1 1  StaZ  |  (4.14) 

These  uncertainties  in  equation  (4.10)  are  eliminated  by  using  equation  (4.6)  but 
with  large  enough  gain  (?/2)  in  order  to  guarantee  stability.  The  boundary  layer 
used  in  numerical  simulation  was  selected  in  order  to  have  the  interpretation  of 
smoothing  out  the  discontinuity  in  the  nominal  control  law  at  the  switching  sur¬ 
face.  The  nominal  control  law  considered  in  this  section, 

u  =  —  [5  rB]  - 1 5  .v  -  [5  r5]  “  sat  sign  ^  ^  (4.15) 

where  ;/:  >  pmsx  \  m,+F{.x)  |  +  |  {[im3X- 1 )  1 1  STAx  \ 
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guarantees  asymptotic  stability  for  the  nonlinear  model  which  have  state  observer 
as  shown  in  Figure  37.  The  nonlinear  feedback  gain  (q2)  was  chosen  according 
to  the  assumed  bounding  nonlinear  terms.  The  selected  gain  (rj2)  in  the  previous 
chapter  is  large  enough  to  handle  any  unknown  bounded  uncertainty.  The  Fig¬ 
ure  37  depicts  the  expected  robustness  of  the  dynamic  response  using  sliding 
mode  compensator.  This  method  produces  an  extremely  robust  sliding  mode 
compensator  that  performs  predictably  despite  the  use  of  the  sliding  surface,  gain 
(//2),  control  law,  and  observer  feedback  gain  (^0)  which  are  based  on  the  nominal 
nlinear  model.  The  FORTRAN  program  "OBSERVER"  in  Appendix  B  has  been 
written  to  implement  the  sliding  mode  compensator.  The  results  of  Figure  37  on 
page  77  were  obtained  by  using  the  standard  hydrodynamic  coefficients  of  the 
nonlinear  model  at  500  rpm. 
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Figure  37.  The  dynamic  response  of  (he  sliding  mode  compensator  for  (he  standard 
AUV 


77 


C.  ROBUSTNESS  TESTS 
1.  Linear  Model 

In  the  previous  numerical  analysis  we  saw  that  sliding  mode  compensator  could 
deal  with  unknown  nonlinear  terms  by  using  nonlinear  feedback  control  law  m. 
A  modified  system  matrix  A,  and  control  matrix  B  is  used  to  estimate  robustness 
performance  of  the  sliding  mode  compensator  in  this  section.  The  effects  of 
modeling  error  in  a  linear  model  can  be  compensated  by  considering  the  size  of 
modeling  error  as  in  the  classical  state  space  methods  [Ref.  11].  The  variations 
in  control  matrix  B  can  be  handled  using  a  similar  method.  This  technique  is  easy 
to  analyze  for  a  single  input  system.  The  effects  of  modeling  error  can  be  consid¬ 
ered  for  any  system  using  sliding  mode  compensator.  Consider  the  effect  of  ? 
variation  in  the  system  matrix  A  and  control  matrix  B.  The  system  dynamics  are 
given  by 


x  =  (A+SA)x+(B+SB)u  (4.16) 

where  5 A  is  the  unknown  bounded  changing  system  matrix 
SB  is  the  unknown  bounded  changing  control  matrix 

The  unknown  changed  system  matrix  (SA)  and  control  (SB)  matrix  can  be  ac¬ 
commodated  by  using  a  large  nonlinear  (?/2)  and  boundary  layer  ((f)).  The  large 
gain  will  guarantee  stability  and  the  boundary  layer  obtained  in  the  previous 
chapter  will  eliminate  the  chattering  which  is  intrinsically  linked  to  the  use  of  a 
switching  surface.  The  FORTRAN  program  "OBSERVER"  in  Appendx  A  is 
used  to  estimate  unmeasurable  state  and  perform  sliding  mode  compensator  to 
the  modified  system.  The  same  boundary  layer  ((f)),  nonlinear  feedback  gain  (rj2), 
control  law  (4.6),  and  observer  gain(/v0)  as  in  the  nominal  system  are  applied  to 
the.  modified  system  to  estimate  the  performance  of  a  sliding  mode  compensator 
designed  on  a  nominal  linear  model.  The  system  matrix  A  and  control  matrix  B 
are  modified  as  in  the  following  Table  6. 
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Table  6.  ROBUSTNESS  TESTS  CASE  OF  THE  SLIDING  MODE 
COMPENSATOR 


Test  No 

System  matrix  A 

Control  matrix  B 

Speed 

Test  1 

2*A 

B 

6  ft/sec 

Test  2 

A/2 

B 

6  ft/sec 

Test  3 

A 

2*B 

6  ft/sec 

Test  4 

A 

B/2 

6  ft/sec 

Test  5 

2*A 

2*B 

6  ft/sec 

Test  6 

A/2 

B/2 

6  ft/sec 

The  dynamic  response  for  each  test  is  shown  in  Figure  38  on  page  80  through 
Figure  43  on  page  85.  Although  the  system  matrix  A  and  control  matrix  B  are 
modified  by  200%,  there  is  only  10  ~  1 1%  overshoot  to  the  test  5.  The  expected, 
robustness  of  the  dynamic  response  is  presented  by  using  sliding  mode 
compensator  based  on  nominal  linear  model.  Sliding  mode  compensator  per¬ 
formance  is  verified  for  a  linear  vehicle  with  uncertain  A  and  B  matrix  without 
considering  additional  design. 

The  sliding  mode  compensator  developed  in  this  section  was  stable  and  insensi¬ 
tive  to  change  in  hydrodynamic  coefficients  of  the  AUV.  It  therefore  appears  that 
variable  structure  systems  will  provide  the  most  robust  design  for  a  sliding  mode 
compensator  that  needs  to  maintain  an  accurate  prediction  of  vehicle  response 
under  varying  conditions. 
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Figure  38.  The  dynamic  response  of  test  1  AUV  (depth  command  =  100  ft) 
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Figure  39.  Tile  dynamic  response  of  test  2  AIJV  (depth  command  =  100  ft) 
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Figure  40.  The  dynamic  response  of  test  3  AUV  (depth  command  =  100  ft) 
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Figure  41.  Thc-dynamic  response  of  test  4  AUV  (depth  command  =  100  ft) 
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Figure  42.  The  dynamic  response  of  tesf  5  AUV  (depth  command  =  100  ft) 
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Figure  43.  The  dynamic  response  of  test  6  AUV  (depth  command  =100  ft) 
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2.  Nonlinear  Model 

The  siding  mode  compensator  based  on  the  nominal  linear  model  is  here  applied 
to  the  highly  nonlinear  vehicle  with  modified  hydrodynamic  coefficients  in  order 
to  estimate  its  robustness. 

The  hydrodynamic  coefficients,  which  affect  in  the  depth  change  maneuvering, 
are  modified  as  Table  5  in  the  previous  chapter.  Although  an  unmodeled  dis¬ 
turbance  could  be  made  up  for  using  model  based  compensator,  parameter  mis¬ 
match  was  determined  to  be  sensitive  to  the  used  method  [Ref.  12].  This  would 
reduce  the  robustness  of  the  controller  when  confronted  with  the  varying,  un¬ 
controlled  condition  found  in  the  ocean  enviroment.  In  this  section,  sliding  mode 
compensator  is  used  to  investigate  robustness  for  control  of  depth  change  ma¬ 
neuvering  in  the  face  of  unmodeled  nonlinear  terms  and  parameter  uncertainty. 
The  nonlinear  system  dynamics  for  this  purpose  were  described  by 

x  -  /ljf+^/(jc'4-jr(x)+[5+55+g(x)]w  (4.17) 

Since  3A,  5B,  J[x ),  and  g(x)  are  unknown  values  in  the  present  system  equation 
described  above,  they  are  assumed  to  be  zero  and  t]2  is  increased  through  numer¬ 
ical  simulation  in  order  to  guarantee  sliding  mode  control  [Ref.  2].  The  nominal 
control  law  (4.6)  is  used  to  simulate  the  response  of  all  values  (q,  u,  z,  and  s)  both 
actual  and  estimated.  The  FORTRAN  program  "NSMC"  was  written  in  order 
to  simulate  numerical  experiments  provided  in  Appendix  B.  Note  that  in  this 
section,  a  soft  saturation  function  might  be  used  to  handle  the  highly  nonlinear 
model  within  the  boundary  layer.  The  soft  saturation  function  is 

S^7=(-^)x^5a(Jc)  (4.18) 

<P 

This  saturation  function  produces  soft  inputs  at  approximately  zero  <r(.x)  values. 
The  dynamic  response  of  the  highly  nonlinear  AUV  is  shown  in  Figure  44  on 
page  87  through  Figure  49  on  page  92. 
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Figure  44.  The  dynamic  response  of  nonlinear  test  1  AUV 
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Figure  45.  Ilie  dynamic  response  of  nonlinear  test  2  AUV 
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Figure  47.  The  dynamic  response  of  nonlinear  test  4  A(JV 
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Figure  48.  Tiie  dynamic  response 
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Figure  49.  The  dynamic  response 
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V.  CONCLUSIONS  AND  RECOMMENDATIONS 


A.  SUMMARY 

This  thesis  presented  an  application  of  variable  structure  systems  for  the  ro¬ 
bust  control  of  linear  and  nonlinear  systems  in  the  presence  of  disturbances  and 
parameter  variations.  The  designed  sliding  mode  compensator  based  on  this 
methodology  was  successfully  employed  to  provide  a  means  for  an  unmanned 
underwater  vehicle  to  control  its  depth  under  high  nonlinearities.  In  this  section, 
we  summarize  the  sliding  mode  compensator  design  of  the  specified  AUV: 

1.  The  fully  nonlinear  equations  of  motion  are  linearized  and  simplified  (as  il¬ 
lustrated  in  section  3.C)  in  order  to  produce  linear  equations  it.  the  state 
space  form. 

2.  The  sliding  surface,  which  provides  desired  dynamics  of  a  closed-loop  sys¬ 
tem,  is  designed  by  the  standard  pole  placement  method  using  the  state 
space  form. 

3.  The  sliding  mode  control  law,  which  consists  of  linear  feedback  and  nonlin¬ 
ear  switching  feedback,  can  be  obtained  by  satisfying  the  sliding  condition. 

4.  Chatfcnn?  due  to  piecewise  discontinuous  feedback  is  eliminated  by  replac¬ 
ing  i  witched  control  law  at  the  sliding  surface  by  continuous  variations 
across  'in  boundary  layer  neighboring  the  switching  surface. 

5.  The  nonlinear  feedback  gain  (?/2)  and  boundary  layer  (<£)  are  optimizeo  by 
a  series  of  numerical,  experiments. 

6.  The  state  observer  is  designed  using  classical  methods. 

7.  The  sliding  mode  compensator  is  designed  by  combining  the  sliding  mode 
control  and  state  observer. 

B.  CONCLUSIONS 

The  objective  of  this  work  to  design  and  analyze  a  sliding  mode  dive  plane 
compensator  for  an  autonomous  underwater  vehicle  has  been  achieved.  Realistic 
limitations  due  to  pitch  and  pitch  rate  sensors  non-availability  were  taken  into 
account.  The  vehicle  that  was  considered,  the  SDV  is  a  typical  AUV  and  its 
shape  and  characteristics  greatly  resemble  the  NPS  vehicle. 

The  conclusions  of  this  work  arc  summarized  below: 
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1.  A  procedure  for  computing  the  sliding  plane  was  established.  The  procedure 
is  very  general  and  can  be  applied  to  a  large  class  of  linearized  single  input 
systems.  Once  the  sliding  plane  coefficients  have  been  determined,  the 
switching  feedback  control  law  follows  easily. 

2.  Chattering  problems,  characteristic  of  variable  structure  system,  were  re¬ 
duced  or  eliminated  by  introducing  a  "boundary  layer"  in  the  switching  logic, 
without  violating  the  sliding  condition. 

3.  Robustness  of  the  design  was  demonstrated  by  a  wide  variation  of  parame¬ 
ters  of  the  linear  model.  Similar  robustness  characteristics  were  established 
when  a  full  order  observer  was  incorporated  in  the  design  in  order  to  provide 
estimates  for  pitch  angle  and  pitch  rate  based  on  depth  measurements  only. 

4.  The  control  design  that  was  based  on  a  linear  model  was  tested  against  the 
full  nonlinear  equations  of  motion.  Numerical  simulations  demonstrated  the 
ability  of  the  design  to  handle  unmodeiled  dynamics  and  variation  of  the 
hydrodynamic  coefficients  and  geometric  parameters  of  the  vehicle. 

5.  A  visual  simulation  using  an  IRIS  graphics  workstation  was  used  [Ref.  13] 
in  order  to  view  the  dynamic  behavior  of  the  AUV  in  real  time  under 
closed-loop  sliding  mode  control.  The  vehicle  response,  Figure  50  on  page 
95,  was  seen  to  follow  the  predictions  that  were  based  on  the  linear  model. 

6.  Finally,  an  experimental  verification  was  attempt  d  on  the  NPS  protype  ve¬ 
hicle  with  coefficients  the  same  as  the  ones  used  in  [Ref.  12  ].  A  discrete¬ 
time  sliding  mode  controller  was  designed  based  on  25  Hz  sample  rate  with 
controller  poles  at  [0.9,0.91, 1],  observer  poles  at  [0.78,0.79,0.80],  control 
law 


u  =  — 0.9$ — 1  .7890—2 1 2916)}2sign{c) 

a  =  —0.33867—1. 68880+(z-zcom)  (5.1) 

where  q2  =  0.2  and  $  =  1 .0 

The  experimental  results  are  shown  in  Figure  51  on  page  96,  where  the 
commanded  depth  was  5  volts  (1  ft  corresponds  to  3.1  volts). 

The  variable  structure  system  was  proven  to  be  an  attractive  control  system  de¬ 
sign  method  for  autonomous  underwater  vehicles.  The  designed  sliding  mode 
compensator  based  on  this  methodology  dealt  with  the  dynamic  problems  of  the 
underwater  vehicle  with  sufficient  accuracy. 
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Figure  50,  Graphic  simulation  display 


C.  RECOMMENDATIONS 


Some  suggestions  for  future  research  are  as  follows: 


1.  Design  and  analyze  a  nonlinear  sliding  mode  compensator. 

2.  Evaluate  the  robustness  characteristics  of  a  digital  (discrete  time)  sliding 
mode  autopilot. 


3.  Extend  the  developed  methodology  in  the  problem  of  path  following 
autopilots  for  the  multiple  input  case  under  constant  or  random  disturbance. 
Introduction  of  integral  control  action  should  help  in  this  case  to  assure 
precise  path  tracking. 
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figure  51.  The  experimental  results  of  the  sliding  mode  compensator 
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APPENDIX  A.  SIMULATION  PROGRAM  AND  BLOCKDIAGRAM  FOR 

LINEAR  EQUATION 


A.  MATRIX-X(SMC)  OF  THE  LINEAR  EQUATION 


****  MATRIX  PROGRAM  FOR  SLIDING  CONTROLLER  *** 

UX-6;  //  FORWARD  SPEED 

CPl-0.25;  //  CLOSED  LOOP  POLE 

CP2-0.27;  //  CLOSED  LOOP  POLE 

51- 1.;  //  SLIDING  SURFACE  COEFFICIENT 

52- ( CP1+CP2 ) ;  //  SLIDING  SURFACE  COEFFICIENT 

53- (CPl*CP2)/(-UX)  //  SLIDING  SURFACE  COEFFICIENT 

T-tOsO. 1:150.]'; 

I ETA  -6.0; 

GA3— 0.035; 

GA4— UX; 

GA5— 0.7; 

GA6— 0.03; 

GA7-S2; 

GA8-S3; 

GA9-IETA; 

GA10-1 ; 

GAl-( GA6-S3*UX)/( -GA3 ) ; 

GA2-  ( GA5+S2  )/( -GA3  ) ; 

INPUT ( : , 1 ) »100*ONES ( T ) ; 

INPUT( : ,2)-0.*ONES(T) ; 

Y«*SIM(T,  INPUT)  ; 

PLOT(T,Y( : ,1) , 'UPPER  LEFT  XLABEL/TIME( SEC )/  YLABEL/. . . 

DEPTH  RESPOSE/TITLE/  DEPTH  COMMAND-  100  , STANDARD  AUV/' ) 
PLOT ( T, Y( : ,2) , 'UPPER  RIGHT  XLABEL/TIME( SEC )/  YLABEL/  ... 

PITCH  ANGLE/TITLE/  CLOSED  LOOP  POLE-O . 0 , -0 . 35 , -0 . 4 0/' ) 

PLOT ( T , Y( : , 4 ) , r  LOWER  LEFT  XLABEL/TIME ( SEC ) /  YLABEL/  ... 

DIVE  PLANE  ANGLE(radins)/TITLE/  SPEED-6/') 

PLOT(T,Y( : ,5) , 'LOWER  RIGHT  XLABEL/TIME ( SEC ) /YLABEL/  ... 
SLIDING  SURFACE/TITLE/  DIS-0.2,  B ,  NLINEAR, IETA-6 . 0 , PI-0 . 4/' ) 


B.  FORTRAN(SMC)  OF  THE  LINEAR  EQUATION 

C  *  LINEAR  AUV  MODEL  * 

C  *  SLIDING  MODE  CONTROL  PROGRAM  * 

C  *  DEPTH  CONTROL  IN  DIVE  PLANE  * 

C  *  BY  USING  RUNGE-KUTTA  FIFTH  ORDER  * 

C 

REAL*8  TIME,QDOT,  ZDOT,  TDOT, THETA, ZP0S,Q,DS 
REAL*8  S,  DE,  UHAT,  UBAR.PHI ,SAT,EITA,COMZ 
C  .:********  INITIAL  CONDITIONS  ********** 

C 

TIME=0.  0D0 
Q=0. 0D0 
THETA=0.  0D0 
ZP0S=0. ODO 

*********  SLIDING  MODE  CONTROL  PARAMETER  *********** 

EITA=4.  ODO 

DELT=0.  02D0 

C0MZ=100. ODO 

PHI=0. 4D0 

Q  *******  SYSTEM  PROGRAM  ******** 

WRITE  (8,710) 

DO  100  1=0,6000 

710  FORMAT  (3X,’  TIME',5X,'  DEPTH',6X,'  DIVE’,5X,' 

*' ,5X, '  SLIDE') 

C 

QD0T=-0.  7D0*Q-0.  03D0*THETA-0. 035D0*DS 
TDOT=Q 

ZD0T=-12.  ODO*THETA 
C 

TIME=TIME+DELT 

Q=Q+DELT*QD0T 

THETA=THETA+DELT*TDOT 

ZPOS=ZPOS+DELT*ZDOT 

S=Q+0.  52D0*THETA-0.  OH2DO*(ZPOS-COMZ) 

IF  (ABS(S)  .LT.  PHI)  SAT=(S/PHI) 

IF  (S  .LT.  -PHI)  SAT=-1. ODO 
IF  (S  .GT.  PHI)  SAT=1.  ODO 
UHAT=-5.  1429D0*Q+1.  0714D0*THETA 
UBAR=EITA*SAT 
DE=UHAT+UBAR 

IF  (DE  .GE.  0. 4D0)  DS=0.4D0 
IF  (DE  .LE.  -0.4D0)  DS=-0. 4D0 
IF  ((DE  .LT.  0.4D0)  .AND.  (DE  .  GT.  -0.4D0))  DS=DE 
WRITE  (8,720)  TIME,  ZPOS,  DS,  THETA,  S 
720  FORMAT  (2X,E11. 3,4X,E11. 3,4X,E11. 3,4X,E11.  3,4X,E11. 3) 

100  CONTINUE 
STOP 
END 
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C.  MATRIX-X(OBSERVER)  OF  THE  LINEAR  EQUATION 


***  SIMULATION  PROGRAM  FOR  SLIDING  MODE  COMPENSATOR  *** 


UX=6 ; 

CPl-0.3 
CP2=0 . 40 ; 

Sl-1. ; 

S2“( CP1+CP2 ) ; 
S3=(CPl*CP2)/(-0.6) ; 
EITA  -6.0; 


//  FORWARD  SPEED 
//  CLOSED  LOOP  POLE 
//  CLOSED  LOOP  POLE 
//  SLIDING  SURFACE  COEFFICIENT 
//  SLIDING  SURFACE  COEFFICIENT 
//  SLIDING  SURFACE  COEFFICIENT 


A- [ -0 . 7 , -0 . 0  3 , 0 ; 1 , 0 , 0 ; 0 , -6 , 0  1 ; 
B=[ -0 . 035 ; 0 ; 0  ] ; 

010,0,1]; 

N-tC' ,A'*C' ,A'*A'*C' ] ; 


W=[ 1,0.7, 0.03;0,1, 0.7 ?0, 0,1]; 

LAl=5.5;  //  DESIRE  POLE  OF  OBSERVER 
LA2*=5 . 75  ;  //  DESIRE  POLE  OF  OBSERVER 
LA3=5 . 9 ;  //  DESIRE  POLE  OF  OBSERVER 


ABAR= (0.7;0.03;0J  ; 

AHAT-[LA1+LA2+LA3;LA1*LA2  +  LA2*LA3+LA1*LA3;LM*LA2*LA3]  ; 
KC— INV ( ( N*W ) ' ) * ( AHAT-ABAR ) ; 

GAl— 1.4; 


GA2“— 0 . 06 ; 


GA3=-2*UX; 
GA4-KC(3,1) ; 
GA5=KC (2,1) ; 
GA6-KC(1,1) ; 
GA7=-0 . 7 ; 
GA8=-0 . 03 ; 
GA9=-6 ; 
GAl0=-0 . 035 ; 


GAl6=-0 . 035 ; 


GAll=0 . 75 ; 

GAl3=-0 . 0233 ; 

GA14= ( GA2-S3*UX)/( -GA10 ) ; 

GA12= ( GA7+S2 )/( -GAlO ) ; 

GA15=EITA; 

T- [0:0. 1:150.]'; 

INPUT( : ,1)  =  100*ONES(T) ; 

INPUT( : , 2 )  =  0 . 0*ONES ( T )  ; 

Y«SIM(T, INPUT) ; 

PLOT( T, [ Y( : , 1 )  Y( : ,5) ] , 'LINE  STYLE  1  2  /UPPER  LEFT  XLABEL/ 
TIME (sec)  /  YLABEL/EST  AND  ACT  DEPTH ( ft)/' ) 

PLOT(T,[Y( : ,2)  Y( : , 4 ) ] , ' LINE  STYLE  1  2  /UPPER  RIGHT  XLABEL/ 
TIME(sec)/  YLABEL/EST  AND  ACT  ANGLE ( rad) /TITLE/  B,  2*A/' ) 
PLOT ( T , Y ( : , 3 ) , ' LOWER  LEFT  XLABEL/TIME( SEC )/  YLABEL/ 

DIVE  PLANE  ANGLE/TITLE/  S.F.C=1,  0.75,  -0.0233,  SPEED=6/' ) 
PLOT(T,Y( : ,6) , 'LOWER  RIGHT  XLABEL/TIME( SEC )/YLABEL/ 

SLIDING  SURFACE/  TITLE/  SAT,  DIS=0.2,  IETA=6 , PI=0 . 4/' ) 
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D.  FORTRAN(OBSERVER)  OF  THE  LINEAR  EQUATION 


**************************************** 


*  LINEAR  AUV  MODEL  * 

*  SLIDING  MODE  CONTROL  PROGRAM  * 

*  DEPTH  CONTROL  IN  DIVE  PLANE  * 

*  BY  USING  EULER -METHOD  * 


***********************  ***************** 


REAL*8  TIME,QD0T,  ZDOT,  TDOT , THETA , ZPOS , Q , DS 
REAL*8  S,  DE,  UHAT,  UBAR,PHI ,SAT,EITA,COMZ 
RE AL*8  THADOT , QHADOT , ZHADOT , QHAT , THAT , ZHAT 

*********  initial  conditions  ********** 


TIME=0.  0D0 
Q=0. 0D0 
THETA=0.  0D0 
ZP0S=0. 0D0 
QHADOT=0.  0D0 
THAD0T=0. 0D0 
ZHAD0T=0. 0D0 
QHAT=0. ODO 
THAT=0. ODO 
ZHAT=0. ODO 

Q  *W«W«'r VfVttWc  SLIDING  MODE  CONTROL  PARAMETER  *********** 

EITA=4. ODO 
DELT=0.  02D0 
C0MZ=100. ODO 
PHI=0. 4D0 

C  *******  SYSTEM  PROGRAM  ******** 

WRITE  (8,710) 

DO  100  1=0,6000 

710  FORMAT  (3X, '  TIME',5X,’  DEPTH’, 6X,'  DIVE',5X,' 

*' ,5X, 1  SLIDE') 

C 

QD0T=-0.  35D0*Q-0.  015D0*THF.TA-0.  0175D0*DS 
TD0T=0. 5D0*Q 
ZDOT=-6. 0D0*THETA 

C 

TIME=TIME+DELT 

Q=Q+DELT*QDOT 

THETA=THETA+DELT’-TDOT 

ZPOS=ZPOS+DELT*ZDOT 

C  ********  SUBROUTINE  OBSERVER  ********* 

CALL  OBSER( QHADOT , THADOT , ZHADOT , QHAT , THAT , ZHAT , ZPOS , DELT) 

C  *******  SLIDING  MODE  INPUT  ********* 


S=0HAT+0.  52D0*THAT-0.  0U2D0*f  ZHAT-COMZ) 
IF  ( ABS(S)  .LT.  PHI)  SAT=(S/PHI) 

IF  (S  .LT.  -PHI)  SAT=-1. ODO 
IF  (S  .GT.  PHI)  SAT=1. ODO 
UHAT=-5.  1429D0*QHAT+1.  0714D0*THAT 


PITCH 
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UBAR=EITA*SAT 

DE=UHAT+UBAR 

IF  (DE  .GE.  0.4D0)  DS=0. 4D0 
IF  (DE  .LE.  -0.4D0)  DS*-0. 4D0 

IF  ((DE  .LT.  0. 4D0)  .AND.  (DE  .  GT.  -0.4D0))  DS=DE 
WRITE  (8,720)  TIME,  ZPOS,  DS,  THETA,  S 
720  FORMAT  (2X,E11.  3,4X,E11. 3,4X,E11. 3,4X,E11. 3,4X,E11.  3) 

100  CONTINUE 
STOP 
END 

C  *********  SUBROUTINE  OBSER  ********** 

C 

SUBROUTINE  OBSER( QHADOT , THADOT , ZHADOT , QHAT , THAT , ZHAT , ZPOS , BELT) 
QHAD0T=-0.  7D0*QHAT-0. 03D0*THAT-20.  9293DO*(ZPOS-ZHAT) 
THAD0T=QHAT-14.  4092D0*( ZPOS -ZHAT) 

ZHAD0T=-6.  0D0*THAT+16.  45D0*( ZPOS -ZHAT) 

QHAT=QHAT+DELT*QHADOT 

THAT=THAT+DELT*THADOT 

ZHAT=ZHAT+DELT*ZHADOT 

RETURN 

END 
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E.  FORTRAN  GRAPH 

DIMENSION  TIME(6000) ,DEPTH(6000) ,DIVE( 6000) ,PITCH( 6000) , 
*SLIDE(6000) 

REAL  TIME, DEPTH, DIVE, PITCH .SLIDE 
DO  1  1=1,6000 

1  READ  (8,*)  TIME(I) ,DEPTH( I) ,DIVE(I) ,PITCH(I) ,SLIDE(I) 
CALL  COMPRS 

C  *****  CREATES  A  DUMMY  PLOT  FOR  POST  PROCESSING****** 

CALL  NOBRDR 
CALL  AREA2D(1.  ,1. ) 

CALL  GRAF(0.  ,1.  ,20.  ,0.  ,1.  ,10) 

CALL  ENDPL(O) 

C  *****  PLOT  COMMAND  DEPTH  ***** 

CALL  PAGE(3. ,4. ) 

CALL  NOBRDR 

CALL  AREA2D(2.  3,3.  0) 

CALL  XNAME('TIME(SEC)$',100) 

CALL  YNAMEC COMMAND  DEPTH(FT)$’ ,100) 

CALL  THKFRMCO.  03) 

CALL  GRAF(0.  ,30.0,120.  ,-5/0,25.0,120.0) 

CALL  THKCRV(0.  02) 

CALL  CURVECTIME, DEPTH, 6000,0) 

CALL  ENDPL(O) 


C  *****  PLOT  DIVE  PLANE  ANGLE  ***** 

CALL  PAGE( 3. ,4. ) 

CALL  NOBRDR 

CALL  AREA2D(2. 3,3.0) 

CALL  XNAME( 'TIME (SEC) $' ,100) 

CALL  YNAMEC DIVE  PLANE  ANGLE(RAD)$’ ,100) 
CALL  THKFRMCO.  03) 

CALL  GRAF(0.  ,30.0,120.  ,-.4,.  2,.  4) 

CALL  THKCRVCO.  02) 

CALL  CURVECTIME, DIVE, 6000,0) 

CALL  ENDPL(O) 


C  *****  PLOT  PITCH  ANGLE  ****** 

CALL  PAGE(3. ,4.  ) 

CALL  NOBRDR 

CALL  AREA2DC2. 3,3.0) 

CALL  XNAMEC ’TIMECSEC)?' , 100) 

CALL  YNAMEC 'PITCH  ANGLEC RAD/SEC) $ ' ,100) 
CALL  THKFRMCO.  03) 

CALL  GRAFCO. ,30. 0,120. ,-l. 0,0.  3,0. 2) 
CALL  THKCRVCO.  02) 

CALL  CURVECTIME, PITCH, 6000,0) 

CALL  ENDPL(O) 


C  *****  PLOT  SLIDING  SURFACE  ***** 

CALL  PAGEC3. ,4.  ) 

CALL  NOBRDR 

CALL  AREA2D(2. 3,3. 0) 

CALL  XNAMEC TIMEC SEC) §' ,100) 

CALL  YNAMEC ' SLIDING  SURFACE( RAD/SEC) $’ , 100) 
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CALL  THKFRM(0.  03) 

CALL  GRAF(0.  ,30.0,120.  ,-0.5,0.  5,1.  5) 
CALL  THKCRV(0. 02) 

CALL  CURVE(TIME, SLIDE, 6000,0) 

CALL  ENDPL(O) 

CALL  DONE PL 

STOP 

END 
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F.  BLOCK  DIAGRAM  OF  THE  MATRIX-X  SIMULATION 


Figure  52.  The  block  diagram  of  the  SMC  simulation 


e  53.  The  block  diagram  of  the 


.simulation 


APPENDIX  B.  SIMULATION  PROGRAM  FOR  NONLINEAR  AUV 


***7V*3V*********&Vc**v*rt***rt**rt***rt***-.V**yr:i!r!lf******5Wcyr********«*'>V* 
*  * 
*  NONLINEAR  AUV  MODEL  /  STERN  PLANE  AND  BOW  PLANE  SEPARATED  * 


* 

* 

VARIABLE  DECLARATION 

11 

**  INPUTS  ** 

248 

* 

* 

* 

CONSTANTS 

70 

PROPULSION  MODEL 

271 

* 

* 

INITIAL  CONDITIONS 

128 

**  OUTPUTS  ** 

423 

* 

* 

MASS  MATRIX 

206 

INTEGRATION 

* 

*  INVERT^MATRIX  2^5****  t*  *y  **  * 


REAL  AW(82,82) 

REAL  MASS , LATYAW , NORPIT 

REAL  MM(6,6) ,G4(4) ,GX4(4) ,BR(4) ,HH(4) 

REAL  B(6,6) ,BB(6 ,6) 

REAL  A( 12,12) ,  AA(12,12) 

REAL  XPP  ,XQQ  ,XRR  ,XPR 
REAL  XUDOT  ,XWQ  ,XVP  ,XVR 
REAL  XQDS  ,XQDB  ,XRDR  ,XW 
REAL  XWW  ,XVDR  ,XWDS  ,XWDB 
REAL  XDSDS.XDBDB  ,XDRDR  ,XQDSN 
REAL  XWDSN  .XDSDSN 

REAL  TIME ,S,EITA, UBAR , UHAT , COMZ , BAR , SIM , DE , SAT 


LATERAL  HYDRODYNAMIC  COEFFICIENTS 


REAL  YPDOT  ,YRDOT,YPQ  ,YQR 
REAL  YVDOT  ,YP  ,YR  ,YVQ 
REAL  YWP  ,YKR  ,YV  ,YW 
REAL  YDR  ,CDY 

NORMAL  HYDRODYNAMIC  COEFFICIENTS 


REAL  ZQDOT  ,ZPP,ZPR  ,ZRR 
REAL  ZWDOT  ,ZQ  ,ZVP  ,ZVR 
REAL  ZW  ,ZVV  , ZDS  >ZDB 
REAL  ZQN  , ZWN  ,ZDSN  ,CDZ 
REAL  ZHADOT.ZHAT 

ROLL  HYDRODYNAMIC  COEFFICIENTS 

REAL  KPDOT  ,KRDOT  ,KPQ  ,KQR 
REAL  KVDOT  ,  KP  ,KR  ,KVQ 
REAL  KWP  ,  KWR  ,KV  ,KVW 
REAL  KPN  ,  KDB 

PITCH  HYDRODYNAMIC  COEFFICIENTS 


REAL  MQDOT  ,MPP  ,MPR,MRR 
REAL  MWDOT  ,  MQ  ,MVP  ,MVR 
REAL  MW  ,  MW  ,MDS  ,MDB 
REAL  MQN  ,  MWN  ,MDSN 
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REAL  QHADOT , QHAT , THADOT , THAT 
YAW  HYDRODYNAMIC  COEFFICIENTS 


REAL  NPDOT , NRDOT , NPQ  ,NQR 
REAL  NVDOT  ,  NP  ,NR  ,NVQ 
REAL  NWP  ,  NWR  ,NV  ,NVW 
REAL  NDR 

MASS  CHARACTERISTICS  OF  THE  FLOODED  VEHICLE 

REAL  WEIGHT  ,  BOY  ,VOL  ,XG 

REAL  YG  ,  ZG  ,XB  ,ZB 

REAL  IX  ,  IY  ,IZ  ,IXZ 

REAL  IYZ  ,  IXY  ,YB 

REAL  L  ,  RHO  ,G  ,NU 

REAL  AO  ,KPROP  ,NPROP  ,  X1TEST 

REAL  DEGRUD  ,DEGSTN 

COMMON  /BLOCK1/  F(12),  FP(6),  XMMINV(6,6),  UCF(4) 
INTEGER  N , I A , IDGT ,  IER , LAST ,  J ,  K,  M ,  JJ ,  KK ,  I 
REAL  WKAREA(54) ,  X(12) 

RUDDER  COEFFICIENTS 


PARAMETER  (  DSMAX=  -0. 175) 


LONGITUDINAL  HYDRODYNAMIC  COEFFICIENTS 


PARAMETERCXPP  = 
&  XUD0T=-7.  6E-3 
&  XQDS=2.  5E-2 
&  XWW  =1.  7E-1 
&  XDSDS=  -l.E-2 
t  XWDSN=3.  5E-3 


7.E-3  ,XQQ  = 
,XWQ  =  -2.E-’ 
,XQDB=-2. 6E- 
,XVDR=1. 7E-3 
, XDBDB=  -8.E-3 
,  XDSDSN=  -1.6E 


1. 


3 


5E-2  ,XRR  =  4.E 
,XVP  *  -3.E-3 
,XRDR=  -l.E-3 
,XWDS=4.  6E-2 
,XDRDR=  -l.E-2 
) 


-3  ,XPR  =7.  5E-4, 
,XVR  =  2.E-2, 
,XVV  =5.  3E-2, 
,XWDB=  l.E-2, 

, XQDSN=  2.E-3, 


LATERAL  HYDRODYNAMIC  COEFFICIENTS 


PARAMETER(YPD0T=1.  2E-4  ,YRD0T=1.  2E-3  ,YPQ  =  4.E-3  ,YQR  =-6.5E-3, 

&  YVD0T=-5. 5E-2  ,YP  =  3.E-3  ,YR=3.E-2  ,YVQ=2.4E-2, 

&  YWP  =2. 3E-1  ,YWR  =-l. 9E-2  ,YV=-1.E-1  ,YVW  =6. 8E-2, 

&  YDR  =2.  7E-2  ,CDY  =3. 5E-1) 

NORMAL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(ZQD0T=-6. 8E-3  ,ZPP  =1. 3E-4  ,ZPR=6.7E-3  ,ZRR  =-7.  4E-3, 

&  ZWD0T=-2..4E-1  ,ZQ  =-l.  4E-1  ,ZVP=-4.8E-2  ,ZVR=4.5E-2, 

&  ZW  =  -3.E-1  ,ZVV=-6.8E-2  ,ZDS=-7.3E-2  ,ZDB  =-2. 6E-2, 

&  ZQN  =*2. 9E-3  ,ZWN  =-5. IE-3  ,ZDSN=  -l.E-2  ,CDZ  =  1.0) 

ROLL  HYDRODYNAMIC  COEFFICIENTS 


PARAMETER( KPDOT=  -l.L-3  ,KRD0T=-3. 


&  KVD0T=1. 3E-4 
&  KWP  =-l.  3E-4 
&  KPN  =-5.  7E-4 


,  KP  =-l.  IE-2 
,  KWR  =1.  4E-2 
,  KDB  =  0.0  ) 


4E-5  ,KPQ  =-6. 9E-5  ,KQR  =1.  7E-2 
KR  =-8. 4E-4  ,KVQ=-5. IE-3, 

KV  =3. IE-3  ,KVW  =-l. 9E-1, 


> 
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PITCH  HYDRODYNAMIC  COEFFICIENTS 


PARAMETER(MQDOT=-l.  7E-2  ,MPP  =5.  3E-5,MPR  =  5.E-3.MRR  =-2. 9E-3, 

&  MWDOT=-6.  8E-3  ,  MQ  =-6. 8E-2  ,MVP  =1. 2E-3  ,MVR  =1.  7E-2, 

6c  MW  =  l.E-1  ,  MW  =-2.  6E-2  ,MDS  =-4. IE-2  ,MDB  =6.  9E-3, 

6e  MQN  =-l.  6E-3  ,  MWN  =-2.  9E-3  ,MDSN  =-5.  2E-3) 

YAW  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(NPD0T=-3.  4E-5  ,NRDOT=-3.  4E-3.NPQ  =-2. IE-2  ,NQR  =2. 7E-3, 
6c  NVDOT=l.  2E-3  ,  NP  =-8.  4E-4  ,NR  =-l.  6E-2  ,NVQ  =  -1.5-2, 

6c  NWP  =-l.  7E-2  ,  NWR  =7. 4E-3  ,NV=-7.4E-3  ,NVW  =-2.  7E-2, 

6c  NDR  =-l.  3E-2) 

MASS  CHARACTERISTICS  OF  THE  FLOODED  VEHICLE 


PARAMETER (  WEIGHT  =12000. ,  BOY  =12000.  ,VOL  =200. 


6c  YG  =  0.  0 
&  IX  =  1500. 

6c  IYZ  =  -10. 

6c  L  =  17.4 
&  AO  =  2.  0 
6c  DEGRUD=  0.  0 


,  ZG  =  0.  2 
,  IY  =  10000. 

,  IXY  =  -10. 

,  RHO  =  1. 94 
,KPR0P  =  0. 
,DEGSTN=  0. 0) 


,XB  =  0. 

,IZ  =  10000. 
,YB  =  0.0, 

,G  =  32.2 
,NPROP  =  0.  , 


,XG  =  0.  , 

,  ZB  =  0.0, 
,IXZ  =  -10.  , 

,NU  =  8.  47E-4  , 
X1TEST=  0. 1  , 


INPUT  INITIAL  CONDITIONS  HERE  IF  REQUIRED 


UO  =  12.0 
VO  =  0. 0 
WO  =  0.  0 
PO  =  0. 0 
QO  =  0.  0 
RO  =  0.0 
PHIO  =  0.0 
THETAO  =  0.  0 
PSIO  =  0. 0 
XPOS0=0. 0 
YPOS0=0. 0 
ZPOS0=0. 0 
DB=  0.  0 
DS  *  0.  0 
DR  =  0. 0 
RPM  =  1000. 0 
LATYAW  =  0. 0 
NORPIT  =  0.  0 
RE  =  UO*L/NU 


U  =  UO 
V  =  VO 
W  =  WO 
P  =  PO 
Q  =  QO 
R  =  RO 

XPOS  =  XPOSO 
YPOS  =  YPOSO 
ZPOS  =  ZPOSO 
PSI  =  PHIO 
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THETA  =  THETAO 
PHI  =  PHIO 
QHAD0T=0. 0 
THAD0T=0. 0 
ZHAD0T=0.  0 
QHAT=0.  0 
THAT=0.  0 
ZHAT=0.  0 

DEFINE  LENGTH  FRACTIONS  FOR  GAUSS  QUADRATURE  TERMS 

G4( 1)  =  0.069431844 
G4(2)  =  0. 330009478 
G4(3)  =  0. 669990521 
G4(4)  -  0.930568155 

DEFINE  WEIGHT  FRACTIONS  FOR  GAUSS  QUADRATURE  TERMS 

GK4( 1)  =  0. 1739274225687 
GK4(2)  =  0.3260725774312 
GK4(3)  =  0.3260725774312 
GK4(4)  =  0. 1739274225687 

DEFINE  THE  BREADTH  BB  AND  HEIGHT  HH  TERMS  FOR  THE  INTEGRATION 

BR( 1)  =  75. 7/12 
BR(2)  =  75. 7/12 
BR(3)  =  75. 7/12 
BR(4)  =  55.08/12 

HH( 1)  =  16.38/12 
HH(2)  =  31.85/12 
HH(3)  =  31.85/12 
HH(4)  =  23. 76/12 

MASS  =  WEIGHT/G 


N  =  6 

DO  15  J  =  1,N 
DO  10  K  =  1,N 
XMM1NV( J,K)  =0.0 
MM(  J,K)  =0.0 
10  CONTINUE 

15  CONTINUE 


MM( 1,1)  =  MASS  ”((RH0/2)*( L**3 ) *XUDOT ) 
MM( 1,5)  =  MASS*ZG 
MM( 1,6)  =  -MASS*YG 
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MM( 2,2)  =  MASS  -((RH0/2)*(L**3)*YVD0T) 

MM(2,4)  =  -MASS*ZG  -((RH0/2)*(L**4)*YPD0T) 

MM(2,6)  =  MASS*XG  -  ((RH0/2)*(L**4)*YRD0T) 

MM(3,3)  =  MASS  -  ((RH0/2)*(L**3)*ZWD0T) 

MM(3,4)  =  MASS*YG 

MM( 3,5)  =  -MASS*XG  -((RH0/2)*(L**4)*ZQD0T) 

MM(4,2)  =  -MASS*ZG  -  ((RH0/2)*(L**4)*KVD0T) 

MM(4,3)  =  MASS*YG  ' 

MM(4,4)  =  IX  -  ((RHO/2)*(L**5)*KPDOT) 

MM( 4,5)  =  -IXY 

MM(4,6)  =  -IXZ  -((RH0/2)*(L**5)*KRD0T) 

MM(5,1)  =  MASS*ZG 

MM( 5,3)  =  -MASS*XG  -((RH0/2)*(L**4)*MWD0T) 

MM(5,4)  =  -IXY 

MM( 5,5)  =  IY  -( (RH0/2)*(L**5)*MQD0T) 

MM(5,6)  =  -IYZ 

MM(6, 1)  =  -MASS*YG 

MM( 6,2)  =  MASS*XG  -((RFC/?)*(L**4)*NVD0T) 

MM(6,4)  =  -IXZ  -  ( (  RK )/ .  »*( L**5 ) *NPDOT) 

MM(6,5)  -  -IYZ 

MM(6,6)  =  IZ  -  ( (RHu/2 )*'( L**5)*NRD0T) 


LAST  =  N*N+3*N 
DO  20  M  =  1 , LAST 
WKAREA(M)  *  0.  0 
20  CONTINUE 

IER  =  0 
IA  =  6 
IDGT  =  4 

CALL  LINV2F( MM , N , I A , XMMINV , IDGT , WKAREA , IER) 


************  INPUTS  ************* 

RUDDER  AND  DIVE  PLANE  COMMANDS 

DELT=0. 1 
SIM=  800.0 
TIME=  0.  0 
DS=  0.0 
DR=  0.0 
DB=  0.0 
RPM=1000. 0 
EITA=6. 0 
COMZ=100. 0 
BAR=0. 4 

c  SIZE  OF  OUTPUT  DATA  ARRAY  FOR  PLOTTING 
c  NUM0UT=6 
c  NUMPN1-4 

c  WRITE  (8,711)  NUMPNT.NUMOUT 
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WRITE  (8,710) 

711  FORMAT  (214) 

DO  100  1=1, SIM 
C 
C 
C 

C  PROPULSION  MODEL 

C 

C 

SIGNU  =  1.  0 

IF  (U.LT.O.  0)  SIGNU  =  -1.0 
IF  (ABS(U).  LT.X1TEST)  U  =  X1TEST 
SIGNN  =  1.  0 

IF  (RPM.LT.  0.0)  SIGNN  =  -1.0 
ETA  =  0.  012*RPM/U 
RE  =  U*L/NU 

CDO  =  .00385  +  (1. 296E-17)*(RE  -  1.  2E7)**2 
CT  =  0.  008*L**2*ETA*ABS ( ETA) / ( AO ) 

CT1  =  0. 008*L**2/(A0) 

EPS  =  -1. 0+SIGNN/SIGNU*(SQRT(CT+l.  0)-l. 0)/(SQRT(CTl+l. 0)-l.  0) 
XPROP  =  CDO*(ETA*ABS(ETA)  -  1. 0) 


CALCULATE  THE  DRAG  FORCE,  INTEGRATE  THE  DRAG  OVER  THE  VEHICLE 
INTEGRATE  USING  A  4  TERM  GAUSS  QUADUTURE 


LATYAW  =  0.  0 
NORPIT  =  0.  0 
DO  500  K  =  1,4 

UCF(K)  =  SQRT((V+G4(K)*R*L)**2  +  (W-G4(K)*Q*L)**2) 

IF(UCF(K). GT. IE-10)  THEN 

TERMO  =  (RH0/2)*(CDY*HH(K)*(V+G4(K)*R*L)**2  + 

&  CDZ*BR(K)*(W-G4(K)*Q*L)**2) 

TERM1  =  TERMO* ( V+G4 ( K ) *R*L) /UCF( K ) 

TERM2  =  TERM0*(W-G4(K)*Q*L)/UCF(K) 

LATYAW  =  LATYAW  +  TERM1*GK4(K)*L 
NORPIT  =  NORPIT  +  TERM2*GK4(K)*L 
END  IF 
500  CONTINUE 
C 

C  FORCE  EQUATIONS 

C 

C 

C  LONGITUDINAL  FORCE 
C 

FP( 1)  =  MASS*V*R  -  MASS*W*Q  +  MASS*XG*Q**2  +  MASS*XG*R**2- 
MASS*YG*P*Q  -  MASS*ZG*P*R  +  (RH0/2)*L**4*(XPP*P**2  + 
XQQ*Q**2  +  XRR*R**2  +  XPR*P*R)  +(RH0/2)*L**3*(XWQ*W*Q  + 
XVP*V*P+XVR*V*R+U*Q* ( XQDS*DS+XQDB*DB ) +XRDR*U*R*DR ) + 
(RH0/2)*L**2*(XVV*V**2  +  XWW*W**2  +  XVDR*U*V*DR  +  U*W* 

( XWDS*DS+XWDB*DB ) +U**2*( XDSDS*DS**2+XDBDB*DB**2+ 
XDRDR*DR**2) ) -(WEIGHT  -BOY)*SIN( THETA)  +(RH0/2)*L**3* 
XQDSN*U*Q*DS*EPS+( RHO/ 2 )*L**2*( XVDSN*U*W*DS+XDSDSN*U**2* 
DS**2)*EPS  +( RHO/ 2 ) *L**2*U**2*XPR0P 
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c 


LATERAL  FORCE 


FP(2)  =  -MASS*U*R  -  MASS*XG*P*Q  +  MASS*YG*R**2  -  MASS*ZG*Q*R  + 

&  ( RHO / 2 )*L**4*( YPQ*P*Q  +  YQR*Q*R) +( RHO/2 )*L**3*( YP*U*P  + 

&  YR*U*R  +  YVQ*V*Q  +  YWP*W*P  +  YWR*W*R)  +  (RHO/2)*L**2* 

&  (YV*U*V  +  YVW*V*W  +YDR*U**2*DR)  -LATYAW  +(WEIGHT-BOY)* 

&  COS(THETA)*SIN( PHI)+MASS*W*P+MASS*YG*P**2 

NORMAL  FORC: 

FP(3)  «  MASS*U*Q  -  MASS*V*P  -  MASS*XG*P*R  -  MASS*YG*Q*R  + 

&  MASS*ZG*P**2  +  MASS*ZG*Q**2  +  (RH0/2)*L**4*(ZPP*P**2  + 

&  ZPR*P*R  +  ZRR*R**2)  +  (RH0/2)*L**3*(ZQ*U*Q  +  ZVF*V*P  + 

&  ZVR*V*R)  +(RHO/2)*L**2*(ZW*U*tf  +  ZVV*V**2  +  U**2*( ZDS* 

&  DS+ZDB*DB))-NORPIT+(WEIGHT-BOY)*COS(THETA)*COS(PHI)+ 

&  ( RHO/2 ) *l**3*ZQN*U*Q*EPS  +(RH0/2)*L**2*( ZWN*U*W  +ZDSN* 

&  U**2*DS)*EPS 

ROLL  FORCE 

FP(4)  =  -IZ*Q*R  +IY*Q*R  -IXY*P*R  +IYZ*Q**2  -IYZ*R**2  +IXZ*P*Q  + 
&  MASS*YG*U*Q  -MASS*YG*V*P  -MASS*ZG*W*P+(RHO/2)*L**5*(KPQ* 

&  P*Q  +  KQR*Q*R)  +(RH0/2)*L**4*(KP*U*P  +KR*U*R  +  KVQ*V*Q  + 

&  KWP*W*P  +  KMR*W*R)  +( RHO/2 )*L**3*(KV*U*V  +  KVW*V*W)  + 

&  (YG*WEIGHT  -  YB*BO Y ) *COS ( THETA ) *COS ( PH I )  -  (ZG*WEIGHT  - 

&  ZB*BOY)*COS(THETA)*SIN( PHI )  +  (RH0/2)*L**4*KPN*U*P*EPS+ 

&  (RH0/2)*L**3*U**2*KPR0P  +MASS*ZG*U*R 

PITCH  FORCE 

FP(S)  =  -IX*P*R  +IZ*P*R  +IXY*Q*R  -IYZ*P*Q  -IXZ*P**2  +IXZ*R**2  - 
&  MASS*XG*U*Q  +  MASS*XG*V*P  +  MASS*ZG*V*R  -  MASS*ZG*W*Q  + 

&  ( RHO/ 2 )*L**5*( MPP*P**2  +MPR*P*R  +MRR*R**2 ) +( RHO / 2 )*L**4* 

&  (MQ*U*Q  +  MVP*V*P  +  MVR*V*R)  +  (RHO/2 ) *L**3* ( MW*U*W  + 

&  MVV*V**2+U**2* ( MDS*DS+MDB*DB ) ) +  NORPIT  -(XG*WEIGHT- 

&  XB*BOY ) *COS ( THETA ) *COS ( PHI ) +( RHO/ 2 ) *L**4*MQN*U*Q*EPS  + 

&  (RHO/  2  )*L**3*(  MWN*U*W+MDSN*U**2*DS  ) *EPS - 

&  ( ZG*WE I GHT- ZB*BOY ) *S IN( THETA) 

YAW  FORCE 

FP(6)  =  -IY*P*Q  +IX*P*Q  +IXY*P**2  -IXY*Q**2  +IYZ*P*R  -IXZ*Q*R  - 
&  MASS*XG*U*R  +  MASS*XG*W*P  -  MASS*YG*V*R  +  MASS*YG*W*Q  + 

&  (RHO/2 ) *L**5*( NPQ*P*Q  +  NQR*Q*R)  +( RHO/2 )*L**4*(NP*U*P+ 

&  NR*U*R  +  NVQ*V*Q  +NWP*W*P  +  NWR*W*R)  +( RHO/2 )*L**3*(NV* 

&  U*V  +  NVW*V*W  +  NDR*U**2*DR)  -  LATYAW  +  (XG*WEIGHT  - 

&  XB*BOY)*COS(THETA)*SIN(PHI)+(YG*WEIGHT)*SIN(THETA) 

&  +(RH0/2)*L**3*U**2*NPR0P-YB*B0Y*SIN(THETA) 


.NOW  COMPUTE  THE  F(l-6)  FUNCTIONS 

DO  600  J  =  1,6 

F(J)  =0.0 
DO  600  K  =  1,6 
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nooon  oooo 


F(J)  =  XMMINV( J,K)*FP(K)  +  F(J) 

600  CONTINUE 

THE  LAST  SIX  EQUATIONS  COME  FROM  THE  KINEMATIC  RELATIONS 

FIRST  SET  THE  DRIFT  CURRENT  VALUES 

UCO  =  0.  0 
VCO  =  0.  0 
WCO  =  0.  0 

INERTIAL  POSITION  RATES  F(7-9) 

F( 7)  =  UCO  +  U*COS(PSI)*COS(THETA)  +  V*(COS(PSI)*SIN(THETA)* 

&  SIN(PHI)  -  SIN(PSI)*COS(PHI))  +  W*(COS(PSI)*SIN(THETA)* 

&  COS(PHI)  +  SIN(PSI)*SIN( PHI)) 

F(8)  =  VCO  +  U*SIN(PSI)*COS( THETA)  +  V*(SIN(PSI)*SIN(THETA)* 

&  SIN(PHI)  +  COS( PSI )*COS( PHI ) )  +  W*(SIN(PSI)*SIN(THETA)* 

&  COS (PHI)  -  COS(PSI)*SIN(PHI)) 

F(9)  =  WCO  -  U*SIN(THETA)  +V*COS(THETA)*SIN(PHI)  4W*C0S( THETA)* 
&  COS(PHI) 

EULER  ANGLE  RATES  F( 10-12) 

F( 10)  =  P  +  Q*SIN( PHI )*TAN( THETA)  +  R*COS(PHI)*TAN(THETA) 

F( 11)  =  Q*COS(PHI)  -  R*SIN(PHI) 

F( 12)  =  Q*SIN(PHI)/COS(THETA)  +  R*COS(PHI)/COS(THETA) 


UDOT  =  F(l) 

VDOT  =  F(2) 

WDOT  =  F(3) 

PDOT  =  F(4) 

QDOT  =  F(5) 

RDOT  =  F( 6) 

XDOT  =  F(7) 

YDOT  =  F(8) 

ZDOT  =  F(9) 

PHIDOT  =  F( 10) 

THETAD  =  F( 11) 

PSIDOT  =  F( 12) 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
CREATE  OUTPUT  DATA  FILE 

TESTER=MOD(FLOAT( I) , 100.  ) 

IF  (TESTER  .EQ.  0.0)  THEN 
TIMER=FLOAT(I)/2. 

WRITE  (8,730)  DS, DR, XPOS,YPOS,ZPOS, ROLL, PITCH, YAW 
WRITE  (8,730)  FP(5) 

WRITE  (8,730)  U,V,W,P,Q,R 
WRITE  (8,730)  IX,IZ,IXY,IYZ,IXZ 
WRITE  (8,730)  MASS,XG,ZG 
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C  WRITE  (8,730)  RHO,L,MPP,MPR,MRR 

C  WRITE  (8,730)  MQ,MVP,MVR,MW 

C  WRITE  (8,730)  MW, MDS,DS,MDB,DB,NORPIT, WEIGHT 
C  WRITE  (8,730)  XB, BOY, THETA, PHI, MQN, EPS 

C  WRITE  (8,730)  MWN,MDSN,ZB 

C  WRITE  (8,730)  TIME, DEPTH, DS, THETA, S 

C20  FORMAT  ( IX ,' TIME ', 3X  ' COMMAND  DEPTH' }3X, 'DIVE  P  ANG* , 

C  *3X,' PITCH  ANGLE' ,3X, 'SLIDING  SURFACE'/) 

C30  FORMAT  (1X,F6. 2,2X,E11.  3.3X.E11.  3,4X,E10.  3.3X.E11.  3) 

710  FORMAT  (IX,'  TIME',3X,'  U',3X,'  ETA',3X,'  XPROP' , 

*3X,  'DS') 

WRITE  (8,730)  TIME, U, ETA, XPROP, DS 
730  FORMAT  (1X,F6. 2,2X,F6.  2,2X,F6.  3,2X,F8.  4,2X,E11.  3) 

C  ENDIF 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  FIRST  ORDER  INTEGRATION 
C 

U  =  U  +  DELT*UDOT 
C 

V  =  V  +  DELT>vVDOT 
C 

W  =  W  +  DELT*WDOT 
c 

P  =  P  +  DELT*PDOT 
C 

Q  =  Q  +  DELT*QDOT 
C 

R  =  R  +  DELT*RDOT 
C 

XPOS  =  XPOS  +  DELT*XDOT 
C 

YPOS  =  YPOS  +  DELT*YDOT 
C 

ZPOS  =  ZPOS  +  DELT*ZDOT 
C 

PHI  =  PHI  +  DELT*PHIDOT 
C 

THETA  =  THETA  +  DELT*THETAD 
C  THETA  =  PITCH 

PS I  =  PS I  +  DELT*PSIDOT 

C  PSI  =  YAW 

C 

C  **********  OBSERVER  OF  SLIDING  CONTROLLER  *********** 

CALL  OB SER( QHADOT , THADOT , ZHADOT , QHAT , THAT , ZHAT , DELT , ZPOS , DS , UO ) 
C 
C 

C  ***********  SLIDING  MODE  CONTROL  INPUT  ************ 

S=QHAT+0.  75*THAT-0.  0233*(ZHAT-C0MZ) 

IF  (ABS(S)  .LT.  BAR)  SAT=(S/BAR)*ABS(S) 

IF  (S  .LE.  -BAR)  SAT=-1 

IF  (S  .GE.  BAR)  SAT=1 

UHAT^l. 4286*QHAT+3.  1429*THAT 

UBAR=EITA*SAT 

DE=UHAT+UBAR 

IF  (DE  .GE.  0.4)  DS=0. 4 


U  =  SURGE  RATE 

V  =  SWAY  RATE 
W  =  HEAVE  RATE 

P  =  ROLL  RATE 
Q  =  PITCH  RATE 
R  *  YAW  RATE 
X  *  SURGE 

Y  =  SWAY 
Z  =  HEAVE 
PHI  =  ROLL 
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IF  (DE  .LT.  -0.4)  DS=-0. 4 
IF(  (DE  .LT.  0.4)  .AND.  (DE  .  GE.  -0.4)  )  DS=DE 
DB=-DS*1. 0 
TIME=TIME+DELT 
C 

PHIANG  =  PHI/0. 0174532925 
THEANG  =  THETA/0.0174532925 
PSIANG  =  PSI/0. 0174532925 
C 

TRAC=-YP0S 

ROLL=PHIANG 

YAW=PSIANG 

DEPTH=-ZP0S 

DEPTH=ZP0S 

PITCH=THEANG 

BOWANG=(DB/.  01745) 

STNANG=(DS/. 01745) 


100  CONTINUE 
STOP 
END 

C  ********  OBSERVER  SUBROUTINE  ******* 

SUBROUTINE  OBSER( QHADOT , THADOT , ZHADOT , QHAT , THAT  s ZHAT , DELT , 
*ZPOS,DS,U) 

C 

QHAD0T=-0.  7*QHAT-0.  03*THAT-0.  035*DS-20.  9293*(ZPOS-ZHAT) 
THADOT=QHAT- 14.  409 2*( ZPOS -ZHAT) 

ZHADOT= - 6*THAT+1 6 .  45*( ZPOS-ZHAT) 

C 

QHAT=QHAT+DELT’vQHADOT 

THAT=THAT+DELT*THADOT 

ZHAT=ZHAT+DELT*ZHADOT 

RETURN 

END 
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